1//
2// Academic License - for use in teaching, academic research, and meeting
3// course requirements at degree granting institutions only. Not for
4// government, commercial, or other organizational use.
5//
6// File: cartesian_trajectory_planner.cpp
7//
8// Code generated for Simulink model 'cartesian_trajectory_planner'.
9//
10// Model version : 1.131
11// Simulink Coder version : 9.3 (R2020a) 18-Nov-2019
12// C/C++ source code generated on : Mon May 25 16:42:52 2020
13//
14// Target selection: ert.tlc
15// Embedded hardware selection: Generic->Unspecified (assume 32-bit Generic)
16// Code generation objectives: Unspecified
17// Validation result: Not run
18//
19#include "cartesian_trajectory_planner.h"
20#include "cartesian_trajectory_planner_private.h"
21
22// Block signals (default storage)
23B_cartesian_trajectory_planne_T cartesian_trajectory_planner_B;
24
25// Continuous states
26X_cartesian_trajectory_planne_T cartesian_trajectory_planner_X;
27
28// Block states (default storage)
29DW_cartesian_trajectory_plann_T cartesian_trajectory_planner_DW;
30
31// Real-time model
32RT_MODEL_cartesian_trajectory_T cartesian_trajectory_planner_M_ =
33 RT_MODEL_cartesian_trajectory_T();
34RT_MODEL_cartesian_trajectory_T *const cartesian_trajectory_planner_M =
35 &cartesian_trajectory_planner_M_;
36
37// Forward declaration for local functions
38static void cartesian_trajec_emxInit_char_T(emxArray_char_T_cartesian_tra_T
39 **pEmxArray, int32_T numDimensions);
40static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_tr_T
41 *pStruct);
42static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
43 *pStruct);
44static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
45 *pStruct);
46static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
47 *pStruct);
48static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
49 *pStruct);
50static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_tra_T
51 *emxArray, int32_T oldNumel);
52static void cartesian_trajec_emxFree_char_T(emxArray_char_T_cartesian_tra_T
53 **pEmxArray);
54static n_robotics_manip_internal_Rig_T *cartesian_t_RigidBody_RigidBody
55 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
56static n_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_b
57 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
58static n_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_bh
59 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
60static n_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_bhp
61 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
62static n_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_bhp3
63 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB);
64static p_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
65 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
66 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
67 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
68 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
69 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
70 n_robotics_manip_internal_Rig_T *iobj_7, B_MATLABSystem_cartesian_traj_T
71 *localB);
72static void cartesian_t_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
73 **pEmxArray, int32_T numDimensions);
74static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
75 *emxArray, int32_T oldNumel);
76static void ca_rigidBodyJoint_get_JointAxis(const
77 c_rigidBodyJoint_cartesian_tr_T *obj, real_T ax[3],
78 B_MATLABSystem_cartesian_traj_T *localB);
79static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
80 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree,
81 B_MATLABSystem_cartesian_traj_T *localB);
82static void cartesian_t_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
83 **pEmxArray);
84static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_tr_T
85 *pStruct);
86static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
87 *pStruct);
88static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
89 *pStruct);
90static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
91 *pStruct);
92static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
93 *pStruct);
94
95// Forward declaration for local functions
96static real_T cartesian_trajectory_pla_norm_l(const real_T x[3]);
97static void cartesian_trajec_emxInit_real_T(emxArray_real_T_cartesian_tra_T
98 **pEmxArray, int32_T numDimensions);
99static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_tra_T
100 *emxArray, int32_T oldNumel);
101static void cartesian_traj_emxInit_char_T_a(emxArray_char_T_cartesian_tra_T
102 **pEmxArray, int32_T numDimensions);
103static void cart_emxEnsureCapacity_char_T_a(emxArray_char_T_cartesian_tra_T
104 *emxArray, int32_T oldNumel);
105static void cartesian_trajec_emxFree_real_T(emxArray_real_T_cartesian_tra_T
106 **pEmxArray);
107static void cartesian_traj_emxFree_char_T_a(emxArray_char_T_cartesian_tra_T
108 **pEmxArray);
109static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
110 f_robotics_manip_internal_IKE_T *iobj_0);
111static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
112 const real_T tform[16], const real_T weights[6]);
113static void RigidBodyTree_validateConfigu_a(x_robotics_manip_internal_Rig_T *obj,
114 real_T Q[6]);
115static boolean_T cartesian_trajectory_pla_strcmp(const
116 emxArray_char_T_cartesian_tra_T *a, const emxArray_char_T_cartesian_tra_T *b);
117static void rigidBodyJoint_get_JointAxis_as(const
118 c_rigidBodyJoint_cartesian_as_T *obj, real_T ax[3]);
119static void cartesian_trajectory_planne_cat(real_T varargin_1, real_T varargin_2,
120 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
121 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9]);
122static void rigidBodyJoint_transformBodyT_a(const
123 c_rigidBodyJoint_cartesian_as_T *obj, const real_T q_data[], const int32_T
124 *q_size, real_T T[16]);
125static void rigidBodyJoint_transformBodyToP(const
126 c_rigidBodyJoint_cartesian_as_T *obj, real_T T[16]);
127static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
128 const real_T qv[6], const emxArray_char_T_cartesian_tra_T *body1Name, real_T
129 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_tra_T *Jac);
130static creal_T cartesian_trajectory_plann_sqrt(const creal_T x);
131static real_T cartesian_trajectory_plan_xnrm2(int32_T n, const real_T x[9],
132 int32_T ix0);
133static real_T cartesian_trajectory_plan_xdotc(int32_T n, const real_T x[9],
134 int32_T ix0, const real_T y[9], int32_T iy0);
135static void cartesian_trajectory_plan_xaxpy(int32_T n, real_T a, int32_T ix0,
136 const real_T y[9], int32_T iy0, real_T b_y[9]);
137static real_T cartesian_trajectory_pl_xnrm2_a(const real_T x[3], int32_T ix0);
138static void cartesian_trajectory__xaxpy_ast(int32_T n, real_T a, const real_T x
139 [9], int32_T ix0, real_T y[3], int32_T iy0);
140static void cartesian_trajectory_p_xaxpy_as(int32_T n, real_T a, const real_T x
141 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9]);
142static void cartesian_trajectory_plan_xswap(const real_T x[9], int32_T ix0,
143 int32_T iy0, real_T b_x[9]);
144static void cartesian_trajectory_plan_xrotg(real_T a, real_T b, real_T *b_a,
145 real_T *b_b, real_T *c, real_T *s);
146static void cartesian_trajectory_plann_xrot(const real_T x[9], int32_T ix0,
147 int32_T iy0, real_T c, real_T s, real_T b_x[9]);
148static void cartesian_trajectory_planne_svd(const real_T A[9], real_T U[9],
149 real_T s[3], real_T V[9]);
150static void cartesian_trajectory_rotm2axang(const real_T R[9], real_T axang[4]);
151static void cartesian_IKHelpers_computeCost(const real_T x[6],
152 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
153 emxArray_real_T_cartesian_tra_T *Jac, f_robotics_manip_internal_IKE_T **b_args);
154static void cartesian_trajectory_planne_eye(real_T b_I[36]);
155static void cartesian_tra_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
156 **pEmxArray, int32_T numDimensions);
157static void cartesian_traje_emxInit_int32_T(emxArray_int32_T_cartesian_tr_T
158 **pEmxArray, int32_T numDimensions);
159static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
160 *emxArray, int32_T oldNumel);
161static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_tr_T
162 *emxArray, int32_T oldNumel);
163static real_T cartesian_trajectory_pla_norm_a(const real_T x[6]);
164static real_T SystemTimeProvider_getElapsedTi(const
165 f_robotics_core_internal_Syst_T *obj);
166static real_T cartesian_trajectory_p_xnrm2_as(int32_T n, const
167 emxArray_real_T_cartesian_tra_T *x, int32_T ix0);
168static void cartesian_trajectory_pla_qrpf_a(const
169 emxArray_real_T_cartesian_tra_T *A, int32_T m, int32_T n,
170 emxArray_real_T_cartesian_tra_T *tau, const emxArray_int32_T_cartesian_tr_T
171 *jpvt, emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T
172 *b_jpvt);
173static void cartesian_trajectory_pl_xzgetrf(int32_T m, int32_T n, const
174 emxArray_real_T_cartesian_tra_T *A, int32_T lda,
175 emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T *ipiv,
176 int32_T *info);
177static void cartesian_trajectory_plan_xtrsm(int32_T m, int32_T n, const
178 emxArray_real_T_cartesian_tra_T *A, int32_T lda, const
179 emxArray_real_T_cartesian_tra_T *B, int32_T ldb,
180 emxArray_real_T_cartesian_tra_T *b_B);
181static void cartesian_traje_emxFree_int32_T(emxArray_int32_T_cartesian_tr_T
182 **pEmxArray);
183static void cartesian_trajectory_p_mldivide(const
184 emxArray_real_T_cartesian_tra_T *A, const emxArray_real_T_cartesian_tra_T *B,
185 emxArray_real_T_cartesian_tra_T *Y);
186static void cartesian_tra_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
187 **pEmxArray);
188static boolean_T DampedBFGSwGradientProjection_a(const
189 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
190 emxArray_real_T_cartesian_tra_T *alpha);
191static void cartesian_trajectory_planne_inv(const
192 emxArray_real_T_cartesian_tra_T *x, emxArray_real_T_cartesian_tra_T *y);
193static void cartesian_trajectory_plann_diag(const
194 emxArray_real_T_cartesian_tra_T *v, emxArray_real_T_cartesian_tra_T *d);
195static void cartesian_trajectory_pl_sqrt_as(emxArray_real_T_cartesian_tra_T *x);
196static boolean_T cartesian_trajectory_planne_any(const
197 emxArray_boolean_T_cartesian__T *x);
198static boolean_T cartesian_tr_isPositiveDefinite(const real_T B[36]);
199static boolean_T DampedBFGSwGradientProjectio_as(const
200 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6]);
201static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
202 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
203 *iter);
204static void cartesian_trajectory_p_isfinite(const
205 emxArray_real_T_cartesian_tra_T *x, emxArray_boolean_T_cartesian__T *b);
206static void cartesi_genrand_uint32_vector_a(uint32_T mt[625], uint32_T u[2]);
207static boolean_T cartesian_trajec_is_valid_state(const uint32_T mt[625]);
208static real_T cartesian_trajectory_genrandu_a(uint32_T mt[625]);
209static real_T cartesia_eml_rand_mt19937ar_ast(uint32_T state[625]);
210static void cartesian_trajectory_plan_randn(const real_T varargin_1[2],
211 emxArray_real_T_cartesian_tra_T *r);
212static void cartesian__eml_rand_mt19937ar_a(const uint32_T state[625], uint32_T
213 b_state[625], real_T *r);
214static void cartesian_trajectory_pla_rand_a(real_T varargin_1,
215 emxArray_real_T_cartesian_tra_T *r);
216static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
217 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
218 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
219 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
220 solutionInfo_Status_size[2]);
221static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
222 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
223 real_T QSol[6]);
224static void cartesian_emxInit_f_cell_wrap_a(emxArray_f_cell_wrap_cartesia_T
225 **pEmxArray, int32_T numDimensions);
226static void emxEnsureCapacity_f_cell_wrap_a(emxArray_f_cell_wrap_cartesia_T
227 *emxArray, int32_T oldNumel);
228static void rigidBodyJoint_get_JointAxis_a(const c_rigidBodyJoint_cartesian__a_T
229 *obj, real_T ax[3]);
230static void RigidBodyTree_forwardKinemati_a(p_robotics_manip_internal_R_a_T *obj,
231 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree);
232static void cartesian_emxFree_f_cell_wrap_a(emxArray_f_cell_wrap_cartesia_T
233 **pEmxArray);
234static void RigidBodyTree_geometricJacobian(p_robotics_manip_internal_R_a_T *obj,
235 const real_T Q[6], emxArray_real_T_cartesian_tra_T *Jac);
236static void matlabCodegenHandle_matla_astwh(ros_slros_internal_block_Subs_T *obj);
237static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj);
238static void matlabCodegenHandle_matlabC_ast(robotics_slmanip_internal__as_T *obj);
239static void cartesian_tr_SystemCore_release(b_inverseKinematics_cartesian_T *obj);
240static void cartesian__SystemCore_delete_as(b_inverseKinematics_cartesian_T *obj);
241static void matlabCodegenHandle_matlabCo_as(b_inverseKinematics_cartesian_T *obj);
242static void emxFreeStruct_c_rigidBodyJoin_a(c_rigidBodyJoint_cartesian_as_T
243 *pStruct);
244static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
245 *pStruct);
246static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
247 *pStruct);
248static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
249 *pStruct);
250static void emxFreeStruct_robotics_slmani_a(robotics_slmanip_internal__as_T
251 *pStruct);
252static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
253 *pStruct);
254static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
255 *pStruct);
256static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
257 *pStruct);
258static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
259 *pStruct);
260static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__a_T
261 *pStruct);
262static void emxFreeStruct_o_robotics_mani_a(o_robotics_manip_internal_R_a_T
263 *pStruct);
264static void emxFreeStruct_p_robotics_mani_a(p_robotics_manip_internal_R_a_T
265 *pStruct);
266static void emxFreeStruct_robotics_slman_as(robotics_slmanip_internal_b_a_T
267 *pStruct);
268static void emxFreeStruct_n_robotics_mani_a(n_robotics_manip_internal_R_a_T
269 *pStruct);
270static void matlabCodegenHandle_matlab_astw(ros_slros_internal_block_Publ_T *obj);
271static void emxInitStruct_c_rigidBodyJoin_a(c_rigidBodyJoint_cartesian_as_T
272 *pStruct);
273static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
274 *pStruct);
275static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
276 *pStruct);
277static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
278 *pStruct);
279static void emxInitStruct_robotics_slmani_a(robotics_slmanip_internal__as_T
280 *pStruct);
281static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
282 *pStruct);
283static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
284 *pStruct);
285static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
286 *pStruct);
287static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
288 *pStruct);
289static void cartesia_twister_state_vector_a(uint32_T mt[625]);
290static void cartesian_tr_eml_rand_mt19937ar(uint32_T state[625]);
291static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_astwhqf2az
292 (v_robotics_manip_internal_Rig_T *obj);
293static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_astwhqf2azt
294 (v_robotics_manip_internal_Rig_T *obj);
295static v_robotics_manip_internal_Rig_T *RigidBody_RigidBod_astwhqf2aztt
296 (v_robotics_manip_internal_Rig_T *obj);
297static v_robotics_manip_internal_Rig_T *RigidBody_RigidBo_astwhqf2azttx
298 (v_robotics_manip_internal_Rig_T *obj);
299static v_robotics_manip_internal_Rig_T *RigidBody_RigidB_astwhqf2azttxa
300 (v_robotics_manip_internal_Rig_T *obj);
301static v_robotics_manip_internal_Rig_T *RigidBody_Rigid_astwhqf2azttxab
302 (v_robotics_manip_internal_Rig_T *obj);
303static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_b
304 (v_robotics_manip_internal_Rig_T *obj);
305static y_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_as
306 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
307 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
308 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
309 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
310 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
311 v_robotics_manip_internal_Rig_T *iobj_7);
312static void cartesian_trajectory_plann_rand(real_T r[5]);
313static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_i
314 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
315static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_l
316 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
317static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_lh
318 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
319static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_k
320 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
321static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_h
322 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0);
323static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
324 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
325 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
326 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
327 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
328 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_as_T
329 *iobj_7, c_rigidBodyJoint_cartesian_as_T *iobj_8,
330 c_rigidBodyJoint_cartesian_as_T *iobj_9, c_rigidBodyJoint_cartesian_as_T
331 *iobj_10, c_rigidBodyJoint_cartesian_as_T *iobj_11,
332 c_rigidBodyJoint_cartesian_as_T *iobj_12, c_rigidBodyJoint_cartesian_as_T
333 *iobj_13, c_rigidBodyJoint_cartesian_as_T *iobj_14,
334 w_robotics_manip_internal_Rig_T *iobj_15);
335static x_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ast
336 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
337 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
338 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
339 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
340 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
341 c_rigidBodyJoint_cartesian_as_T *iobj_7, c_rigidBodyJoint_cartesian_as_T
342 *iobj_8, c_rigidBodyJoint_cartesian_as_T *iobj_9,
343 c_rigidBodyJoint_cartesian_as_T *iobj_10, c_rigidBodyJoint_cartesian_as_T
344 *iobj_11, c_rigidBodyJoint_cartesian_as_T *iobj_12,
345 c_rigidBodyJoint_cartesian_as_T *iobj_13, c_rigidBodyJoint_cartesian_as_T
346 *iobj_14, c_rigidBodyJoint_cartesian_as_T *iobj_15,
347 w_robotics_manip_internal_Rig_T *iobj_16);
348static c_rigidBodyJoint_cartesian_as_T *c_rigidBodyJoint_rigidBodyJoint
349 (c_rigidBodyJoint_cartesian_as_T *obj, const emxArray_char_T_cartesian_tra_T
350 *jname, const emxArray_char_T_cartesian_tra_T *jtype);
351static w_robotics_manip_internal_Rig_T *cartesian_trajec_RigidBody_copy(const
352 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0,
353 c_rigidBodyJoint_cartesian_as_T *iobj_1, w_robotics_manip_internal_Rig_T
354 *iobj_2);
355static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
356 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_tra_T
357 *parentName, c_rigidBodyJoint_cartesian_as_T *iobj_0,
358 c_rigidBodyJoint_cartesian_as_T *iobj_1, w_robotics_manip_internal_Rig_T
359 *iobj_2);
360static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
361 y_robotics_manip_internal_Rig_T *rigidbodytree,
362 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
363 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
364 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
365 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
366 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
367 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
368 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
369 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
370 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
371 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
372 c_rigidBodyJoint_cartesian_as_T *iobj_15, c_rigidBodyJoint_cartesian_as_T
373 *iobj_16, c_rigidBodyJoint_cartesian_as_T *iobj_17,
374 c_rigidBodyJoint_cartesian_as_T *iobj_18, c_rigidBodyJoint_cartesian_as_T
375 *iobj_19, c_rigidBodyJoint_cartesian_as_T *iobj_20,
376 c_rigidBodyJoint_cartesian_as_T *iobj_21, c_rigidBodyJoint_cartesian_as_T
377 *iobj_22, c_rigidBodyJoint_cartesian_as_T *iobj_23,
378 c_rigidBodyJoint_cartesian_as_T *iobj_24, c_rigidBodyJoint_cartesian_as_T
379 *iobj_25, c_rigidBodyJoint_cartesian_as_T *iobj_26,
380 c_rigidBodyJoint_cartesian_as_T *iobj_27, c_rigidBodyJoint_cartesian_as_T
381 *iobj_28, c_rigidBodyJoint_cartesian_as_T *iobj_29,
382 c_rigidBodyJoint_cartesian_as_T *iobj_30, c_rigidBodyJoint_cartesian_as_T
383 *iobj_31, c_rigidBodyJoint_cartesian_as_T *iobj_32,
384 c_rigidBodyJoint_cartesian_as_T *iobj_33, c_rigidBodyJoint_cartesian_as_T
385 *iobj_34, c_rigidBodyJoint_cartesian_as_T *iobj_35,
386 c_rigidBodyJoint_cartesian_as_T *iobj_36, c_rigidBodyJoint_cartesian_as_T
387 *iobj_37, c_rigidBodyJoint_cartesian_as_T *iobj_38,
388 c_rigidBodyJoint_cartesian_as_T *iobj_39, w_robotics_manip_internal_Rig_T
389 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41);
390static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
391 (h_robotics_core_internal_Damp_T *obj);
392static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__a_T
393 *pStruct);
394static void emxInitStruct_o_robotics_mani_a(o_robotics_manip_internal_R_a_T
395 *pStruct);
396static void emxInitStruct_p_robotics_mani_a(p_robotics_manip_internal_R_a_T
397 *pStruct);
398static void emxInitStruct_robotics_slman_as(robotics_slmanip_internal_b_a_T
399 *pStruct);
400static void emxInitStruct_n_robotics_mani_a(n_robotics_manip_internal_R_a_T
401 *pStruct);
402static n_robotics_manip_internal_R_a_T *cartesian_RigidBody_RigidBody_a
403 (n_robotics_manip_internal_R_a_T *obj);
404static n_robotics_manip_internal_R_a_T *cartesia_RigidBody_RigidBody_as
405 (n_robotics_manip_internal_R_a_T *obj);
406static n_robotics_manip_internal_R_a_T *cartesi_RigidBody_RigidBody_ast
407 (n_robotics_manip_internal_R_a_T *obj);
408static n_robotics_manip_internal_R_a_T *cartes_RigidBody_RigidBody_astw
409 (n_robotics_manip_internal_R_a_T *obj);
410static n_robotics_manip_internal_R_a_T *carte_RigidBody_RigidBody_astwh
411 (n_robotics_manip_internal_R_a_T *obj);
412static n_robotics_manip_internal_R_a_T *cart_RigidBody_RigidBody_astwhq
413 (n_robotics_manip_internal_R_a_T *obj);
414static n_robotics_manip_internal_R_a_T *car_RigidBody_RigidBody_astwhqf
415 (n_robotics_manip_internal_R_a_T *obj);
416static n_robotics_manip_internal_R_a_T *ca_RigidBody_RigidBody_astwhqf2
417 (n_robotics_manip_internal_R_a_T *obj);
418static o_robotics_manip_internal_R_a_T *c_RigidBody_RigidBody_astwhqf2a
419 (o_robotics_manip_internal_R_a_T *obj);
420static p_robotics_manip_internal_R_a_T *c_RigidBodyTree_RigidBodyTree_a
421 (p_robotics_manip_internal_R_a_T *obj, n_robotics_manip_internal_R_a_T *iobj_0,
422 n_robotics_manip_internal_R_a_T *iobj_1, n_robotics_manip_internal_R_a_T
423 *iobj_2, n_robotics_manip_internal_R_a_T *iobj_3,
424 n_robotics_manip_internal_R_a_T *iobj_4, n_robotics_manip_internal_R_a_T
425 *iobj_5, n_robotics_manip_internal_R_a_T *iobj_6,
426 n_robotics_manip_internal_R_a_T *iobj_7);
427int32_T div_s32_floor(int32_T numerator, int32_T denominator)
428{
429 int32_T quotient;
430 uint32_T absNumerator;
431 uint32_T absDenominator;
432 uint32_T tempAbsQuotient;
433 boolean_T quotientNeedsNegation;
434 if (denominator == 0) {
435 quotient = numerator >= 0 ? MAX_int32_T : MIN_int32_T;
436
437 // Divide by zero handler
438 } else {
439 absNumerator = numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :
440 static_cast<uint32_T>(numerator);
441 absDenominator = denominator < 0 ? ~static_cast<uint32_T>(denominator) + 1U :
442 static_cast<uint32_T>(denominator);
443 quotientNeedsNegation = ((numerator < 0) != (denominator < 0));
444 tempAbsQuotient = absNumerator / absDenominator;
445 if (quotientNeedsNegation) {
446 absNumerator %= absDenominator;
447 if (absNumerator > 0U) {
448 tempAbsQuotient++;
449 }
450 }
451
452 quotient = quotientNeedsNegation ? -static_cast<int32_T>(tempAbsQuotient) :
453 static_cast<int32_T>(tempAbsQuotient);
454 }
455
456 return quotient;
457}
458
459//
460// This function updates continuous states using the ODE3 fixed-step
461// solver algorithm
462//
463static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
464{
465 // Solver Matrices
466 static const real_T rt_ODE3_A[3] = {
467 1.0/2.0, 3.0/4.0, 1.0
468 };
469
470 static const real_T rt_ODE3_B[3][3] = {
471 { 1.0/2.0, 0.0, 0.0 },
472
473 { 0.0, 3.0/4.0, 0.0 },
474
475 { 2.0/9.0, 1.0/3.0, 4.0/9.0 }
476 };
477
478 time_T t = rtsiGetT(si);
479 time_T tnew = rtsiGetSolverStopTime(si);
480 time_T h = rtsiGetStepSize(si);
481 real_T *x = rtsiGetContStates(si);
482 ODE3_IntgData *id = static_cast<ODE3_IntgData *>(rtsiGetSolverData(si));
483 real_T *y = id->y;
484 real_T *f0 = id->f[0];
485 real_T *f1 = id->f[1];
486 real_T *f2 = id->f[2];
487 real_T hB[3];
488 int_T i;
489 int_T nXc = 6;
490 rtsiSetSimTimeStep(si,MINOR_TIME_STEP);
491
492 // Save the state values at time t in y, we'll use x as ynew.
493 (void) memcpy(y, x,
494 static_cast<uint_T>(nXc)*sizeof(real_T));
495
496 // Assumes that rtsiSetT and ModelOutputs are up-to-date
497 // f0 = f(t,y)
498 rtsiSetdX(si, f0);
499 cartesian_trajectory_planner_derivatives();
500
501 // f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*));
502 hB[0] = h * rt_ODE3_B[0][0];
503 for (i = 0; i < nXc; i++) {
504 x[i] = y[i] + (f0[i]*hB[0]);
505 }
506
507 rtsiSetT(si, t + h*rt_ODE3_A[0]);
508 rtsiSetdX(si, f1);
509 cartesian_trajectory_planner_step();
510 cartesian_trajectory_planner_derivatives();
511
512 // f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*));
513 for (i = 0; i <= 1; i++) {
514 hB[i] = h * rt_ODE3_B[1][i];
515 }
516
517 for (i = 0; i < nXc; i++) {
518 x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]);
519 }
520
521 rtsiSetT(si, t + h*rt_ODE3_A[1]);
522 rtsiSetdX(si, f2);
523 cartesian_trajectory_planner_step();
524 cartesian_trajectory_planner_derivatives();
525
526 // tnew = t + hA(3);
527 // ynew = y + f*hB(:,3);
528 for (i = 0; i <= 2; i++) {
529 hB[i] = h * rt_ODE3_B[2][i];
530 }
531
532 for (i = 0; i < nXc; i++) {
533 x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]);
534 }
535
536 rtsiSetT(si, tnew);
537 rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
538}
539
540static void cartesian_trajec_emxInit_char_T(emxArray_char_T_cartesian_tra_T
541 **pEmxArray, int32_T numDimensions)
542{
543 emxArray_char_T_cartesian_tra_T *emxArray;
544 int32_T i;
545 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)malloc(sizeof
546 (emxArray_char_T_cartesian_tra_T));
547 emxArray = *pEmxArray;
548 emxArray->data = (char_T *)NULL;
549 emxArray->numDimensions = numDimensions;
550 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
551 emxArray->allocatedSize = 0;
552 emxArray->canFreeData = true;
553 for (i = 0; i < numDimensions; i++) {
554 emxArray->size[i] = 0;
555 }
556}
557
558static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_tr_T
559 *pStruct)
560{
561 cartesian_trajec_emxInit_char_T(&pStruct->Type, 2);
562}
563
564static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
565 *pStruct)
566{
567 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
568 emxInitStruct_c_rigidBodyJoint(&pStruct->JointInternal);
569}
570
571static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
572 *pStruct)
573{
574 emxInitStruct_o_robotics_manip_(&pStruct->Base);
575}
576
577static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
578 *pStruct)
579{
580 emxInitStruct_p_robotics_manip_(&pStruct->TreeInternal);
581}
582
583static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
584 *pStruct)
585{
586 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
587 emxInitStruct_c_rigidBodyJoint(&pStruct->JointInternal);
588}
589
590static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_tra_T
591 *emxArray, int32_T oldNumel)
592{
593 int32_T newNumel;
594 int32_T i;
595 void *newData;
596 if (oldNumel < 0) {
597 oldNumel = 0;
598 }
599
600 newNumel = 1;
601 for (i = 0; i < emxArray->numDimensions; i++) {
602 newNumel *= emxArray->size[i];
603 }
604
605 if (newNumel > emxArray->allocatedSize) {
606 i = emxArray->allocatedSize;
607 if (i < 16) {
608 i = 16;
609 }
610
611 while (i < newNumel) {
612 if (i > 1073741823) {
613 i = MAX_int32_T;
614 } else {
615 i <<= 1;
616 }
617 }
618
619 newData = calloc(static_cast<uint32_T>(i), sizeof(char_T));
620 if (emxArray->data != NULL) {
621 memcpy(newData, emxArray->data, sizeof(char_T) * oldNumel);
622 if (emxArray->canFreeData) {
623 free(emxArray->data);
624 }
625 }
626
627 emxArray->data = (char_T *)newData;
628 emxArray->allocatedSize = i;
629 emxArray->canFreeData = true;
630 }
631}
632
633static void cartesian_trajec_emxFree_char_T(emxArray_char_T_cartesian_tra_T
634 **pEmxArray)
635{
636 if (*pEmxArray != (emxArray_char_T_cartesian_tra_T *)NULL) {
637 if (((*pEmxArray)->data != (char_T *)NULL) && (*pEmxArray)->canFreeData) {
638 free((*pEmxArray)->data);
639 }
640
641 free((*pEmxArray)->size);
642 free(*pEmxArray);
643 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)NULL;
644 }
645}
646
647static n_robotics_manip_internal_Rig_T *cartesian_t_RigidBody_RigidBody
648 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
649{
650 n_robotics_manip_internal_Rig_T *b_obj;
651 emxArray_char_T_cartesian_tra_T *switch_expression;
652 boolean_T b_bool;
653 int32_T b_kstr;
654 int32_T loop_ub;
655 static const char_T tmp[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
656 'l', 'i', 'n', 'k' };
657
658 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
659
660 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
661
662 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
663
664 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
665 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
666
667 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
668 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
669
670 int32_T exitg1;
671 b_obj = obj;
672 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
673 obj->NameInternal->size[0] = 1;
674 obj->NameInternal->size[1] = 13;
675 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
676 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
677 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
678 }
679
680 obj->ParentIndex = 0.0;
681 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
682 obj->JointInternal.Type->size[0] = 1;
683 obj->JointInternal.Type->size[1] = 5;
684 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
685 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
686 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
687 }
688
689 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
690 b_kstr = switch_expression->size[0] * switch_expression->size[1];
691 switch_expression->size[0] = 1;
692 switch_expression->size[1] = obj->JointInternal.Type->size[1];
693 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
694 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
695 - 1;
696 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
697 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
698 }
699
700 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
701 localB->b_d[b_kstr] = tmp_1[b_kstr];
702 }
703
704 b_bool = false;
705 if (switch_expression->size[1] == 8) {
706 b_kstr = 1;
707 do {
708 exitg1 = 0;
709 if (b_kstr - 1 < 8) {
710 loop_ub = b_kstr - 1;
711 if (switch_expression->data[loop_ub] != localB->b_d[loop_ub]) {
712 exitg1 = 1;
713 } else {
714 b_kstr++;
715 }
716 } else {
717 b_bool = true;
718 exitg1 = 1;
719 }
720 } while (exitg1 == 0);
721 }
722
723 if (b_bool) {
724 b_kstr = 0;
725 } else {
726 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
727 localB->b_f[b_kstr] = tmp_2[b_kstr];
728 }
729
730 b_bool = false;
731 if (switch_expression->size[1] == 9) {
732 b_kstr = 1;
733 do {
734 exitg1 = 0;
735 if (b_kstr - 1 < 9) {
736 loop_ub = b_kstr - 1;
737 if (switch_expression->data[loop_ub] != localB->b_f[loop_ub]) {
738 exitg1 = 1;
739 } else {
740 b_kstr++;
741 }
742 } else {
743 b_bool = true;
744 exitg1 = 1;
745 }
746 } while (exitg1 == 0);
747 }
748
749 if (b_bool) {
750 b_kstr = 1;
751 } else {
752 b_kstr = -1;
753 }
754 }
755
756 cartesian_trajec_emxFree_char_T(&switch_expression);
757 switch (b_kstr) {
758 case 0:
759 obj->JointInternal.PositionNumber = 1.0;
760 obj->JointInternal.JointAxisInternal[0] = 0.0;
761 obj->JointInternal.JointAxisInternal[1] = 0.0;
762 obj->JointInternal.JointAxisInternal[2] = 1.0;
763 break;
764
765 case 1:
766 obj->JointInternal.PositionNumber = 1.0;
767 obj->JointInternal.JointAxisInternal[0] = 0.0;
768 obj->JointInternal.JointAxisInternal[1] = 0.0;
769 obj->JointInternal.JointAxisInternal[2] = 1.0;
770 break;
771
772 default:
773 obj->JointInternal.PositionNumber = 0.0;
774 obj->JointInternal.JointAxisInternal[0] = 0.0;
775 obj->JointInternal.JointAxisInternal[1] = 0.0;
776 obj->JointInternal.JointAxisInternal[2] = 0.0;
777 break;
778 }
779
780 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
781 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
782 }
783
784 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
785 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
786 }
787
788 obj->JointInternal.JointAxisInternal[0] = 0.0;
789 obj->JointInternal.JointAxisInternal[1] = 0.0;
790 obj->JointInternal.JointAxisInternal[2] = 0.0;
791 return b_obj;
792}
793
794static n_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_b
795 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
796{
797 n_robotics_manip_internal_Rig_T *b_obj;
798 emxArray_char_T_cartesian_tra_T *switch_expression;
799 boolean_T b_bool;
800 int32_T b_kstr;
801 int32_T loop_ub;
802 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
803 '1' };
804
805 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
806
807 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
808
809 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
810 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
811
812 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
813 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
814
815 int32_T exitg1;
816 b_obj = obj;
817 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
818 obj->NameInternal->size[0] = 1;
819 obj->NameInternal->size[1] = 10;
820 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
821 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
822 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
823 }
824
825 obj->ParentIndex = 1.0;
826 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
827 obj->JointInternal.Type->size[0] = 1;
828 obj->JointInternal.Type->size[1] = 8;
829 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
830 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
831 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
832 }
833
834 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
835 b_kstr = switch_expression->size[0] * switch_expression->size[1];
836 switch_expression->size[0] = 1;
837 switch_expression->size[1] = obj->JointInternal.Type->size[1];
838 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
839 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
840 - 1;
841 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
842 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
843 }
844
845 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
846 localB->b_j[b_kstr] = tmp_0[b_kstr];
847 }
848
849 b_bool = false;
850 if (switch_expression->size[1] == 8) {
851 b_kstr = 1;
852 do {
853 exitg1 = 0;
854 if (b_kstr - 1 < 8) {
855 loop_ub = b_kstr - 1;
856 if (switch_expression->data[loop_ub] != localB->b_j[loop_ub]) {
857 exitg1 = 1;
858 } else {
859 b_kstr++;
860 }
861 } else {
862 b_bool = true;
863 exitg1 = 1;
864 }
865 } while (exitg1 == 0);
866 }
867
868 if (b_bool) {
869 b_kstr = 0;
870 } else {
871 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
872 localB->b_cv[b_kstr] = tmp_1[b_kstr];
873 }
874
875 b_bool = false;
876 if (switch_expression->size[1] == 9) {
877 b_kstr = 1;
878 do {
879 exitg1 = 0;
880 if (b_kstr - 1 < 9) {
881 loop_ub = b_kstr - 1;
882 if (switch_expression->data[loop_ub] != localB->b_cv[loop_ub]) {
883 exitg1 = 1;
884 } else {
885 b_kstr++;
886 }
887 } else {
888 b_bool = true;
889 exitg1 = 1;
890 }
891 } while (exitg1 == 0);
892 }
893
894 if (b_bool) {
895 b_kstr = 1;
896 } else {
897 b_kstr = -1;
898 }
899 }
900
901 cartesian_trajec_emxFree_char_T(&switch_expression);
902 switch (b_kstr) {
903 case 0:
904 obj->JointInternal.PositionNumber = 1.0;
905 obj->JointInternal.JointAxisInternal[0] = 0.0;
906 obj->JointInternal.JointAxisInternal[1] = 0.0;
907 obj->JointInternal.JointAxisInternal[2] = 1.0;
908 break;
909
910 case 1:
911 obj->JointInternal.PositionNumber = 1.0;
912 obj->JointInternal.JointAxisInternal[0] = 0.0;
913 obj->JointInternal.JointAxisInternal[1] = 0.0;
914 obj->JointInternal.JointAxisInternal[2] = 1.0;
915 break;
916
917 default:
918 obj->JointInternal.PositionNumber = 0.0;
919 obj->JointInternal.JointAxisInternal[0] = 0.0;
920 obj->JointInternal.JointAxisInternal[1] = 0.0;
921 obj->JointInternal.JointAxisInternal[2] = 0.0;
922 break;
923 }
924
925 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
926 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
927 }
928
929 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
930 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
931 }
932
933 obj->JointInternal.JointAxisInternal[0] = 0.0;
934 obj->JointInternal.JointAxisInternal[1] = 0.0;
935 obj->JointInternal.JointAxisInternal[2] = 1.0;
936 return b_obj;
937}
938
939static n_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_bh
940 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
941{
942 n_robotics_manip_internal_Rig_T *b_obj;
943 emxArray_char_T_cartesian_tra_T *switch_expression;
944 boolean_T b_bool;
945 int32_T b_kstr;
946 int32_T loop_ub;
947 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
948 '2' };
949
950 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
951
952 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
953
954 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
955 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
956 0.0, 0.0, 0.0, 1.0 };
957
958 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
959 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
960
961 int32_T exitg1;
962 b_obj = obj;
963 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
964 obj->NameInternal->size[0] = 1;
965 obj->NameInternal->size[1] = 10;
966 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
967 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
968 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
969 }
970
971 obj->ParentIndex = 2.0;
972 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
973 obj->JointInternal.Type->size[0] = 1;
974 obj->JointInternal.Type->size[1] = 8;
975 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
976 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
977 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
978 }
979
980 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
981 b_kstr = switch_expression->size[0] * switch_expression->size[1];
982 switch_expression->size[0] = 1;
983 switch_expression->size[1] = obj->JointInternal.Type->size[1];
984 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
985 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
986 - 1;
987 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
988 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
989 }
990
991 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
992 localB->b_l[b_kstr] = tmp_0[b_kstr];
993 }
994
995 b_bool = false;
996 if (switch_expression->size[1] == 8) {
997 b_kstr = 1;
998 do {
999 exitg1 = 0;
1000 if (b_kstr - 1 < 8) {
1001 loop_ub = b_kstr - 1;
1002 if (switch_expression->data[loop_ub] != localB->b_l[loop_ub]) {
1003 exitg1 = 1;
1004 } else {
1005 b_kstr++;
1006 }
1007 } else {
1008 b_bool = true;
1009 exitg1 = 1;
1010 }
1011 } while (exitg1 == 0);
1012 }
1013
1014 if (b_bool) {
1015 b_kstr = 0;
1016 } else {
1017 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1018 localB->b_p[b_kstr] = tmp_1[b_kstr];
1019 }
1020
1021 b_bool = false;
1022 if (switch_expression->size[1] == 9) {
1023 b_kstr = 1;
1024 do {
1025 exitg1 = 0;
1026 if (b_kstr - 1 < 9) {
1027 loop_ub = b_kstr - 1;
1028 if (switch_expression->data[loop_ub] != localB->b_p[loop_ub]) {
1029 exitg1 = 1;
1030 } else {
1031 b_kstr++;
1032 }
1033 } else {
1034 b_bool = true;
1035 exitg1 = 1;
1036 }
1037 } while (exitg1 == 0);
1038 }
1039
1040 if (b_bool) {
1041 b_kstr = 1;
1042 } else {
1043 b_kstr = -1;
1044 }
1045 }
1046
1047 cartesian_trajec_emxFree_char_T(&switch_expression);
1048 switch (b_kstr) {
1049 case 0:
1050 obj->JointInternal.PositionNumber = 1.0;
1051 obj->JointInternal.JointAxisInternal[0] = 0.0;
1052 obj->JointInternal.JointAxisInternal[1] = 0.0;
1053 obj->JointInternal.JointAxisInternal[2] = 1.0;
1054 break;
1055
1056 case 1:
1057 obj->JointInternal.PositionNumber = 1.0;
1058 obj->JointInternal.JointAxisInternal[0] = 0.0;
1059 obj->JointInternal.JointAxisInternal[1] = 0.0;
1060 obj->JointInternal.JointAxisInternal[2] = 1.0;
1061 break;
1062
1063 default:
1064 obj->JointInternal.PositionNumber = 0.0;
1065 obj->JointInternal.JointAxisInternal[0] = 0.0;
1066 obj->JointInternal.JointAxisInternal[1] = 0.0;
1067 obj->JointInternal.JointAxisInternal[2] = 0.0;
1068 break;
1069 }
1070
1071 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1072 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
1073 }
1074
1075 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1076 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1077 }
1078
1079 obj->JointInternal.JointAxisInternal[0] = 0.0;
1080 obj->JointInternal.JointAxisInternal[1] = 0.0;
1081 obj->JointInternal.JointAxisInternal[2] = -1.0;
1082 return b_obj;
1083}
1084
1085static n_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_bhp
1086 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
1087{
1088 n_robotics_manip_internal_Rig_T *b_obj;
1089 emxArray_char_T_cartesian_tra_T *switch_expression;
1090 boolean_T b_bool;
1091 int32_T b_kstr;
1092 int32_T loop_ub;
1093 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1094 '3' };
1095
1096 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1097
1098 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1099
1100 static const real_T tmp_2[16] = { 1.0, 2.0682310711021444E-13,
1101 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
1102 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
1103 1.0 };
1104
1105 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1106 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
1107
1108 int32_T exitg1;
1109 b_obj = obj;
1110 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
1111 obj->NameInternal->size[0] = 1;
1112 obj->NameInternal->size[1] = 10;
1113 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
1114 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
1115 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
1116 }
1117
1118 obj->ParentIndex = 3.0;
1119 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
1120 obj->JointInternal.Type->size[0] = 1;
1121 obj->JointInternal.Type->size[1] = 8;
1122 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
1123 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1124 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
1125 }
1126
1127 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
1128 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1129 switch_expression->size[0] = 1;
1130 switch_expression->size[1] = obj->JointInternal.Type->size[1];
1131 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
1132 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
1133 - 1;
1134 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1135 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
1136 }
1137
1138 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1139 localB->b_pp[b_kstr] = tmp_0[b_kstr];
1140 }
1141
1142 b_bool = false;
1143 if (switch_expression->size[1] == 8) {
1144 b_kstr = 1;
1145 do {
1146 exitg1 = 0;
1147 if (b_kstr - 1 < 8) {
1148 loop_ub = b_kstr - 1;
1149 if (switch_expression->data[loop_ub] != localB->b_pp[loop_ub]) {
1150 exitg1 = 1;
1151 } else {
1152 b_kstr++;
1153 }
1154 } else {
1155 b_bool = true;
1156 exitg1 = 1;
1157 }
1158 } while (exitg1 == 0);
1159 }
1160
1161 if (b_bool) {
1162 b_kstr = 0;
1163 } else {
1164 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1165 localB->b_b[b_kstr] = tmp_1[b_kstr];
1166 }
1167
1168 b_bool = false;
1169 if (switch_expression->size[1] == 9) {
1170 b_kstr = 1;
1171 do {
1172 exitg1 = 0;
1173 if (b_kstr - 1 < 9) {
1174 loop_ub = b_kstr - 1;
1175 if (switch_expression->data[loop_ub] != localB->b_b[loop_ub]) {
1176 exitg1 = 1;
1177 } else {
1178 b_kstr++;
1179 }
1180 } else {
1181 b_bool = true;
1182 exitg1 = 1;
1183 }
1184 } while (exitg1 == 0);
1185 }
1186
1187 if (b_bool) {
1188 b_kstr = 1;
1189 } else {
1190 b_kstr = -1;
1191 }
1192 }
1193
1194 cartesian_trajec_emxFree_char_T(&switch_expression);
1195 switch (b_kstr) {
1196 case 0:
1197 obj->JointInternal.PositionNumber = 1.0;
1198 obj->JointInternal.JointAxisInternal[0] = 0.0;
1199 obj->JointInternal.JointAxisInternal[1] = 0.0;
1200 obj->JointInternal.JointAxisInternal[2] = 1.0;
1201 break;
1202
1203 case 1:
1204 obj->JointInternal.PositionNumber = 1.0;
1205 obj->JointInternal.JointAxisInternal[0] = 0.0;
1206 obj->JointInternal.JointAxisInternal[1] = 0.0;
1207 obj->JointInternal.JointAxisInternal[2] = 1.0;
1208 break;
1209
1210 default:
1211 obj->JointInternal.PositionNumber = 0.0;
1212 obj->JointInternal.JointAxisInternal[0] = 0.0;
1213 obj->JointInternal.JointAxisInternal[1] = 0.0;
1214 obj->JointInternal.JointAxisInternal[2] = 0.0;
1215 break;
1216 }
1217
1218 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1219 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
1220 }
1221
1222 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1223 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1224 }
1225
1226 obj->JointInternal.JointAxisInternal[0] = 0.0;
1227 obj->JointInternal.JointAxisInternal[1] = 0.0;
1228 obj->JointInternal.JointAxisInternal[2] = 1.0;
1229 return b_obj;
1230}
1231
1232static n_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_bhp3
1233 (n_robotics_manip_internal_Rig_T *obj, B_MATLABSystem_cartesian_traj_T *localB)
1234{
1235 n_robotics_manip_internal_Rig_T *b_obj;
1236 emxArray_char_T_cartesian_tra_T *switch_expression;
1237 boolean_T b_bool;
1238 int32_T b_kstr;
1239 int32_T loop_ub;
1240 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1241 '4' };
1242
1243 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1244
1245 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1246
1247 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
1248 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
1249 0.0, -0.268, 0.0, 1.0 };
1250
1251 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1252 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
1253
1254 int32_T exitg1;
1255 b_obj = obj;
1256 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
1257 obj->NameInternal->size[0] = 1;
1258 obj->NameInternal->size[1] = 10;
1259 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
1260 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
1261 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
1262 }
1263
1264 obj->ParentIndex = 4.0;
1265 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
1266 obj->JointInternal.Type->size[0] = 1;
1267 obj->JointInternal.Type->size[1] = 8;
1268 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
1269 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1270 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
1271 }
1272
1273 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
1274 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1275 switch_expression->size[0] = 1;
1276 switch_expression->size[1] = obj->JointInternal.Type->size[1];
1277 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
1278 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
1279 - 1;
1280 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1281 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
1282 }
1283
1284 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1285 localB->b_n[b_kstr] = tmp_0[b_kstr];
1286 }
1287
1288 b_bool = false;
1289 if (switch_expression->size[1] == 8) {
1290 b_kstr = 1;
1291 do {
1292 exitg1 = 0;
1293 if (b_kstr - 1 < 8) {
1294 loop_ub = b_kstr - 1;
1295 if (switch_expression->data[loop_ub] != localB->b_n[loop_ub]) {
1296 exitg1 = 1;
1297 } else {
1298 b_kstr++;
1299 }
1300 } else {
1301 b_bool = true;
1302 exitg1 = 1;
1303 }
1304 } while (exitg1 == 0);
1305 }
1306
1307 if (b_bool) {
1308 b_kstr = 0;
1309 } else {
1310 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1311 localB->b_cx[b_kstr] = tmp_1[b_kstr];
1312 }
1313
1314 b_bool = false;
1315 if (switch_expression->size[1] == 9) {
1316 b_kstr = 1;
1317 do {
1318 exitg1 = 0;
1319 if (b_kstr - 1 < 9) {
1320 loop_ub = b_kstr - 1;
1321 if (switch_expression->data[loop_ub] != localB->b_cx[loop_ub]) {
1322 exitg1 = 1;
1323 } else {
1324 b_kstr++;
1325 }
1326 } else {
1327 b_bool = true;
1328 exitg1 = 1;
1329 }
1330 } while (exitg1 == 0);
1331 }
1332
1333 if (b_bool) {
1334 b_kstr = 1;
1335 } else {
1336 b_kstr = -1;
1337 }
1338 }
1339
1340 cartesian_trajec_emxFree_char_T(&switch_expression);
1341 switch (b_kstr) {
1342 case 0:
1343 obj->JointInternal.PositionNumber = 1.0;
1344 obj->JointInternal.JointAxisInternal[0] = 0.0;
1345 obj->JointInternal.JointAxisInternal[1] = 0.0;
1346 obj->JointInternal.JointAxisInternal[2] = 1.0;
1347 break;
1348
1349 case 1:
1350 obj->JointInternal.PositionNumber = 1.0;
1351 obj->JointInternal.JointAxisInternal[0] = 0.0;
1352 obj->JointInternal.JointAxisInternal[1] = 0.0;
1353 obj->JointInternal.JointAxisInternal[2] = 1.0;
1354 break;
1355
1356 default:
1357 obj->JointInternal.PositionNumber = 0.0;
1358 obj->JointInternal.JointAxisInternal[0] = 0.0;
1359 obj->JointInternal.JointAxisInternal[1] = 0.0;
1360 obj->JointInternal.JointAxisInternal[2] = 0.0;
1361 break;
1362 }
1363
1364 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1365 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
1366 }
1367
1368 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1369 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1370 }
1371
1372 obj->JointInternal.JointAxisInternal[0] = 0.0;
1373 obj->JointInternal.JointAxisInternal[1] = 0.0;
1374 obj->JointInternal.JointAxisInternal[2] = 1.0;
1375 return b_obj;
1376}
1377
1378static p_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
1379 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
1380 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
1381 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
1382 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
1383 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
1384 n_robotics_manip_internal_Rig_T *iobj_7, B_MATLABSystem_cartesian_traj_T
1385 *localB)
1386{
1387 p_robotics_manip_internal_Rig_T *b_obj;
1388 o_robotics_manip_internal_Rig_T *obj_0;
1389 emxArray_char_T_cartesian_tra_T *switch_expression;
1390 boolean_T b_bool;
1391 int32_T b_kstr;
1392 int32_T loop_ub;
1393 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1394 '5' };
1395
1396 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1397
1398 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1399
1400 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1401 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
1402
1403 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1404 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
1405
1406 static const char_T tmp_4[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1407 '6' };
1408
1409 static const real_T tmp_5[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1410 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
1411
1412 static const char_T tmp_6[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1413 'e', 'e' };
1414
1415 static const char_T tmp_7[5] = { 'f', 'i', 'x', 'e', 'd' };
1416
1417 static const char_T tmp_8[5] = { 'w', 'o', 'r', 'l', 'd' };
1418
1419 int32_T exitg1;
1420 b_obj = obj;
1421 obj->Bodies[0] = cartesian_t_RigidBody_RigidBody(iobj_0, localB);
1422 obj->Bodies[1] = cartesian_RigidBody_RigidBody_b(iobj_7, localB);
1423 obj->Bodies[2] = cartesia_RigidBody_RigidBody_bh(iobj_1, localB);
1424 obj->Bodies[3] = cartesi_RigidBody_RigidBody_bhp(iobj_2, localB);
1425 obj->Bodies[4] = cartes_RigidBody_RigidBody_bhp3(iobj_3, localB);
1426 b_kstr = iobj_4->NameInternal->size[0] * iobj_4->NameInternal->size[1];
1427 iobj_4->NameInternal->size[0] = 1;
1428 iobj_4->NameInternal->size[1] = 10;
1429 cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal, b_kstr);
1430 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
1431 iobj_4->NameInternal->data[b_kstr] = tmp[b_kstr];
1432 }
1433
1434 iobj_4->ParentIndex = 5.0;
1435 b_kstr = iobj_4->JointInternal.Type->size[0] * iobj_4->
1436 JointInternal.Type->size[1];
1437 iobj_4->JointInternal.Type->size[0] = 1;
1438 iobj_4->JointInternal.Type->size[1] = 8;
1439 cartes_emxEnsureCapacity_char_T(iobj_4->JointInternal.Type, b_kstr);
1440 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1441 iobj_4->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
1442 }
1443
1444 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
1445 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1446 switch_expression->size[0] = 1;
1447 switch_expression->size[1] = iobj_4->JointInternal.Type->size[1];
1448 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
1449 loop_ub = iobj_4->JointInternal.Type->size[0] * iobj_4->
1450 JointInternal.Type->size[1] - 1;
1451 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1452 switch_expression->data[b_kstr] = iobj_4->JointInternal.Type->data[b_kstr];
1453 }
1454
1455 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1456 localB->b_m[b_kstr] = tmp_0[b_kstr];
1457 }
1458
1459 b_bool = false;
1460 if (switch_expression->size[1] == 8) {
1461 b_kstr = 1;
1462 do {
1463 exitg1 = 0;
1464 if (b_kstr - 1 < 8) {
1465 loop_ub = b_kstr - 1;
1466 if (switch_expression->data[loop_ub] != localB->b_m[loop_ub]) {
1467 exitg1 = 1;
1468 } else {
1469 b_kstr++;
1470 }
1471 } else {
1472 b_bool = true;
1473 exitg1 = 1;
1474 }
1475 } while (exitg1 == 0);
1476 }
1477
1478 if (b_bool) {
1479 b_kstr = 0;
1480 } else {
1481 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1482 localB->b_k[b_kstr] = tmp_1[b_kstr];
1483 }
1484
1485 b_bool = false;
1486 if (switch_expression->size[1] == 9) {
1487 b_kstr = 1;
1488 do {
1489 exitg1 = 0;
1490 if (b_kstr - 1 < 9) {
1491 loop_ub = b_kstr - 1;
1492 if (switch_expression->data[loop_ub] != localB->b_k[loop_ub]) {
1493 exitg1 = 1;
1494 } else {
1495 b_kstr++;
1496 }
1497 } else {
1498 b_bool = true;
1499 exitg1 = 1;
1500 }
1501 } while (exitg1 == 0);
1502 }
1503
1504 if (b_bool) {
1505 b_kstr = 1;
1506 } else {
1507 b_kstr = -1;
1508 }
1509 }
1510
1511 switch (b_kstr) {
1512 case 0:
1513 iobj_4->JointInternal.PositionNumber = 1.0;
1514 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
1515 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
1516 iobj_4->JointInternal.JointAxisInternal[2] = 1.0;
1517 break;
1518
1519 case 1:
1520 iobj_4->JointInternal.PositionNumber = 1.0;
1521 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
1522 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
1523 iobj_4->JointInternal.JointAxisInternal[2] = 1.0;
1524 break;
1525
1526 default:
1527 iobj_4->JointInternal.PositionNumber = 0.0;
1528 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
1529 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
1530 iobj_4->JointInternal.JointAxisInternal[2] = 0.0;
1531 break;
1532 }
1533
1534 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1535 iobj_4->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
1536 }
1537
1538 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1539 iobj_4->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1540 }
1541
1542 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
1543 iobj_4->JointInternal.JointAxisInternal[1] = 1.0;
1544 iobj_4->JointInternal.JointAxisInternal[2] = 0.0;
1545 obj->Bodies[5] = iobj_4;
1546 b_kstr = iobj_5->NameInternal->size[0] * iobj_5->NameInternal->size[1];
1547 iobj_5->NameInternal->size[0] = 1;
1548 iobj_5->NameInternal->size[1] = 10;
1549 cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal, b_kstr);
1550 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
1551 iobj_5->NameInternal->data[b_kstr] = tmp_4[b_kstr];
1552 }
1553
1554 iobj_5->ParentIndex = 6.0;
1555 b_kstr = iobj_5->JointInternal.Type->size[0] * iobj_5->
1556 JointInternal.Type->size[1];
1557 iobj_5->JointInternal.Type->size[0] = 1;
1558 iobj_5->JointInternal.Type->size[1] = 8;
1559 cartes_emxEnsureCapacity_char_T(iobj_5->JointInternal.Type, b_kstr);
1560 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1561 iobj_5->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
1562 }
1563
1564 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1565 switch_expression->size[0] = 1;
1566 switch_expression->size[1] = iobj_5->JointInternal.Type->size[1];
1567 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
1568 loop_ub = iobj_5->JointInternal.Type->size[0] * iobj_5->
1569 JointInternal.Type->size[1] - 1;
1570 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1571 switch_expression->data[b_kstr] = iobj_5->JointInternal.Type->data[b_kstr];
1572 }
1573
1574 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1575 localB->b_m[b_kstr] = tmp_0[b_kstr];
1576 }
1577
1578 b_bool = false;
1579 if (switch_expression->size[1] == 8) {
1580 b_kstr = 1;
1581 do {
1582 exitg1 = 0;
1583 if (b_kstr - 1 < 8) {
1584 loop_ub = b_kstr - 1;
1585 if (switch_expression->data[loop_ub] != localB->b_m[loop_ub]) {
1586 exitg1 = 1;
1587 } else {
1588 b_kstr++;
1589 }
1590 } else {
1591 b_bool = true;
1592 exitg1 = 1;
1593 }
1594 } while (exitg1 == 0);
1595 }
1596
1597 if (b_bool) {
1598 b_kstr = 0;
1599 } else {
1600 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1601 localB->b_k[b_kstr] = tmp_1[b_kstr];
1602 }
1603
1604 b_bool = false;
1605 if (switch_expression->size[1] == 9) {
1606 b_kstr = 1;
1607 do {
1608 exitg1 = 0;
1609 if (b_kstr - 1 < 9) {
1610 loop_ub = b_kstr - 1;
1611 if (switch_expression->data[loop_ub] != localB->b_k[loop_ub]) {
1612 exitg1 = 1;
1613 } else {
1614 b_kstr++;
1615 }
1616 } else {
1617 b_bool = true;
1618 exitg1 = 1;
1619 }
1620 } while (exitg1 == 0);
1621 }
1622
1623 if (b_bool) {
1624 b_kstr = 1;
1625 } else {
1626 b_kstr = -1;
1627 }
1628 }
1629
1630 switch (b_kstr) {
1631 case 0:
1632 iobj_5->JointInternal.PositionNumber = 1.0;
1633 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
1634 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
1635 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
1636 break;
1637
1638 case 1:
1639 iobj_5->JointInternal.PositionNumber = 1.0;
1640 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
1641 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
1642 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
1643 break;
1644
1645 default:
1646 iobj_5->JointInternal.PositionNumber = 0.0;
1647 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
1648 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
1649 iobj_5->JointInternal.JointAxisInternal[2] = 0.0;
1650 break;
1651 }
1652
1653 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1654 iobj_5->JointInternal.JointToParentTransform[b_kstr] = tmp_5[b_kstr];
1655 }
1656
1657 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1658 iobj_5->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1659 }
1660
1661 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
1662 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
1663 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
1664 obj->Bodies[6] = iobj_5;
1665 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
1666 iobj_6->NameInternal->size[0] = 1;
1667 iobj_6->NameInternal->size[1] = 11;
1668 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);
1669 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
1670 iobj_6->NameInternal->data[b_kstr] = tmp_6[b_kstr];
1671 }
1672
1673 iobj_6->ParentIndex = 7.0;
1674 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
1675 JointInternal.Type->size[1];
1676 iobj_6->JointInternal.Type->size[0] = 1;
1677 iobj_6->JointInternal.Type->size[1] = 5;
1678 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);
1679 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
1680 iobj_6->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];
1681 }
1682
1683 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1684 switch_expression->size[0] = 1;
1685 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
1686 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
1687 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
1688 JointInternal.Type->size[1] - 1;
1689 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1690 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
1691 }
1692
1693 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1694 localB->b_m[b_kstr] = tmp_0[b_kstr];
1695 }
1696
1697 b_bool = false;
1698 if (switch_expression->size[1] == 8) {
1699 b_kstr = 1;
1700 do {
1701 exitg1 = 0;
1702 if (b_kstr - 1 < 8) {
1703 loop_ub = b_kstr - 1;
1704 if (switch_expression->data[loop_ub] != localB->b_m[loop_ub]) {
1705 exitg1 = 1;
1706 } else {
1707 b_kstr++;
1708 }
1709 } else {
1710 b_bool = true;
1711 exitg1 = 1;
1712 }
1713 } while (exitg1 == 0);
1714 }
1715
1716 if (b_bool) {
1717 b_kstr = 0;
1718 } else {
1719 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1720 localB->b_k[b_kstr] = tmp_1[b_kstr];
1721 }
1722
1723 b_bool = false;
1724 if (switch_expression->size[1] == 9) {
1725 b_kstr = 1;
1726 do {
1727 exitg1 = 0;
1728 if (b_kstr - 1 < 9) {
1729 loop_ub = b_kstr - 1;
1730 if (switch_expression->data[loop_ub] != localB->b_k[loop_ub]) {
1731 exitg1 = 1;
1732 } else {
1733 b_kstr++;
1734 }
1735 } else {
1736 b_bool = true;
1737 exitg1 = 1;
1738 }
1739 } while (exitg1 == 0);
1740 }
1741
1742 if (b_bool) {
1743 b_kstr = 1;
1744 } else {
1745 b_kstr = -1;
1746 }
1747 }
1748
1749 switch (b_kstr) {
1750 case 0:
1751 iobj_6->JointInternal.PositionNumber = 1.0;
1752 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
1753 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
1754 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
1755 break;
1756
1757 case 1:
1758 iobj_6->JointInternal.PositionNumber = 1.0;
1759 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
1760 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
1761 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
1762 break;
1763
1764 default:
1765 iobj_6->JointInternal.PositionNumber = 0.0;
1766 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
1767 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
1768 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
1769 break;
1770 }
1771
1772 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1773 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
1774 }
1775
1776 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1777 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
1778 }
1779
1780 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
1781 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
1782 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
1783 obj->Bodies[7] = iobj_6;
1784 obj->NumBodies = 8.0;
1785 obj->PositionNumber = 6.0;
1786 obj_0 = &obj->Base;
1787 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
1788 obj->Base.NameInternal->size[0] = 1;
1789 obj->Base.NameInternal->size[1] = 5;
1790 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);
1791 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
1792 obj->Base.NameInternal->data[b_kstr] = tmp_8[b_kstr];
1793 }
1794
1795 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
1796 Base.JointInternal.Type->size[1];
1797 obj->Base.JointInternal.Type->size[0] = 1;
1798 obj->Base.JointInternal.Type->size[1] = 5;
1799 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);
1800 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
1801 obj_0->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];
1802 }
1803
1804 b_kstr = switch_expression->size[0] * switch_expression->size[1];
1805 switch_expression->size[0] = 1;
1806 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
1807 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
1808 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
1809 Base.JointInternal.Type->size[1] - 1;
1810 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1811 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
1812 }
1813
1814 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1815 localB->b_m[b_kstr] = tmp_0[b_kstr];
1816 }
1817
1818 b_bool = false;
1819 if (switch_expression->size[1] == 8) {
1820 b_kstr = 1;
1821 do {
1822 exitg1 = 0;
1823 if (b_kstr - 1 < 8) {
1824 loop_ub = b_kstr - 1;
1825 if (switch_expression->data[loop_ub] != localB->b_m[loop_ub]) {
1826 exitg1 = 1;
1827 } else {
1828 b_kstr++;
1829 }
1830 } else {
1831 b_bool = true;
1832 exitg1 = 1;
1833 }
1834 } while (exitg1 == 0);
1835 }
1836
1837 if (b_bool) {
1838 b_kstr = 0;
1839 } else {
1840 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
1841 localB->b_k[b_kstr] = tmp_1[b_kstr];
1842 }
1843
1844 b_bool = false;
1845 if (switch_expression->size[1] == 9) {
1846 b_kstr = 1;
1847 do {
1848 exitg1 = 0;
1849 if (b_kstr - 1 < 9) {
1850 loop_ub = b_kstr - 1;
1851 if (switch_expression->data[loop_ub] != localB->b_k[loop_ub]) {
1852 exitg1 = 1;
1853 } else {
1854 b_kstr++;
1855 }
1856 } else {
1857 b_bool = true;
1858 exitg1 = 1;
1859 }
1860 } while (exitg1 == 0);
1861 }
1862
1863 if (b_bool) {
1864 b_kstr = 1;
1865 } else {
1866 b_kstr = -1;
1867 }
1868 }
1869
1870 cartesian_trajec_emxFree_char_T(&switch_expression);
1871 switch (b_kstr) {
1872 case 0:
1873 obj->Base.JointInternal.PositionNumber = 1.0;
1874 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
1875 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
1876 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
1877 break;
1878
1879 case 1:
1880 obj->Base.JointInternal.PositionNumber = 1.0;
1881 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
1882 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
1883 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
1884 break;
1885
1886 default:
1887 obj->Base.JointInternal.PositionNumber = 0.0;
1888 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
1889 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
1890 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
1891 break;
1892 }
1893
1894 return b_obj;
1895}
1896
1897static void cartesian_t_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
1898 **pEmxArray, int32_T numDimensions)
1899{
1900 emxArray_f_cell_wrap_cartesia_T *emxArray;
1901 int32_T i;
1902 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)malloc(sizeof
1903 (emxArray_f_cell_wrap_cartesia_T));
1904 emxArray = *pEmxArray;
1905 emxArray->data = (f_cell_wrap_cartesian_traject_T *)NULL;
1906 emxArray->numDimensions = numDimensions;
1907 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
1908 emxArray->allocatedSize = 0;
1909 emxArray->canFreeData = true;
1910 for (i = 0; i < numDimensions; i++) {
1911 emxArray->size[i] = 0;
1912 }
1913}
1914
1915static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
1916 *emxArray, int32_T oldNumel)
1917{
1918 int32_T newNumel;
1919 int32_T i;
1920 void *newData;
1921 if (oldNumel < 0) {
1922 oldNumel = 0;
1923 }
1924
1925 newNumel = 1;
1926 for (i = 0; i < emxArray->numDimensions; i++) {
1927 newNumel *= emxArray->size[i];
1928 }
1929
1930 if (newNumel > emxArray->allocatedSize) {
1931 i = emxArray->allocatedSize;
1932 if (i < 16) {
1933 i = 16;
1934 }
1935
1936 while (i < newNumel) {
1937 if (i > 1073741823) {
1938 i = MAX_int32_T;
1939 } else {
1940 i <<= 1;
1941 }
1942 }
1943
1944 newData = calloc(static_cast<uint32_T>(i), sizeof
1945 (f_cell_wrap_cartesian_traject_T));
1946 if (emxArray->data != NULL) {
1947 memcpy(newData, emxArray->data, sizeof(f_cell_wrap_cartesian_traject_T)
1948 * oldNumel);
1949 if (emxArray->canFreeData) {
1950 free(emxArray->data);
1951 }
1952 }
1953
1954 emxArray->data = (f_cell_wrap_cartesian_traject_T *)newData;
1955 emxArray->allocatedSize = i;
1956 emxArray->canFreeData = true;
1957 }
1958}
1959
1960static void ca_rigidBodyJoint_get_JointAxis(const
1961 c_rigidBodyJoint_cartesian_tr_T *obj, real_T ax[3],
1962 B_MATLABSystem_cartesian_traj_T *localB)
1963{
1964 boolean_T b_bool;
1965 emxArray_char_T_cartesian_tra_T *a;
1966 int32_T b_kstr;
1967 int32_T loop_ub;
1968 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1969
1970 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1971
1972 boolean_T guard1 = false;
1973 int32_T exitg1;
1974 cartesian_trajec_emxInit_char_T(&a, 2);
1975 b_kstr = a->size[0] * a->size[1];
1976 a->size[0] = 1;
1977 a->size[1] = obj->Type->size[1];
1978 cartes_emxEnsureCapacity_char_T(a, b_kstr);
1979 loop_ub = obj->Type->size[0] * obj->Type->size[1] - 1;
1980 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1981 a->data[b_kstr] = obj->Type->data[b_kstr];
1982 }
1983
1984 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
1985 localB->b_g1[b_kstr] = tmp[b_kstr];
1986 }
1987
1988 b_bool = false;
1989 if (a->size[1] == 8) {
1990 b_kstr = 1;
1991 do {
1992 exitg1 = 0;
1993 if (b_kstr - 1 < 8) {
1994 loop_ub = b_kstr - 1;
1995 if (a->data[loop_ub] != localB->b_g1[loop_ub]) {
1996 exitg1 = 1;
1997 } else {
1998 b_kstr++;
1999 }
2000 } else {
2001 b_bool = true;
2002 exitg1 = 1;
2003 }
2004 } while (exitg1 == 0);
2005 }
2006
2007 guard1 = false;
2008 if (b_bool) {
2009 guard1 = true;
2010 } else {
2011 b_kstr = a->size[0] * a->size[1];
2012 a->size[0] = 1;
2013 a->size[1] = obj->Type->size[1];
2014 cartes_emxEnsureCapacity_char_T(a, b_kstr);
2015 loop_ub = obj->Type->size[0] * obj->Type->size[1] - 1;
2016 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2017 a->data[b_kstr] = obj->Type->data[b_kstr];
2018 }
2019
2020 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
2021 localB->b_c[b_kstr] = tmp_0[b_kstr];
2022 }
2023
2024 b_bool = false;
2025 if (a->size[1] == 9) {
2026 b_kstr = 1;
2027 do {
2028 exitg1 = 0;
2029 if (b_kstr - 1 < 9) {
2030 loop_ub = b_kstr - 1;
2031 if (a->data[loop_ub] != localB->b_c[loop_ub]) {
2032 exitg1 = 1;
2033 } else {
2034 b_kstr++;
2035 }
2036 } else {
2037 b_bool = true;
2038 exitg1 = 1;
2039 }
2040 } while (exitg1 == 0);
2041 }
2042
2043 if (b_bool) {
2044 guard1 = true;
2045 } else {
2046 ax[0] = (rtNaN);
2047 ax[1] = (rtNaN);
2048 ax[2] = (rtNaN);
2049 }
2050 }
2051
2052 if (guard1) {
2053 ax[0] = obj->JointAxisInternal[0];
2054 ax[1] = obj->JointAxisInternal[1];
2055 ax[2] = obj->JointAxisInternal[2];
2056 }
2057
2058 cartesian_trajec_emxFree_char_T(&a);
2059}
2060
2061static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
2062 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree,
2063 B_MATLABSystem_cartesian_traj_T *localB)
2064{
2065 n_robotics_manip_internal_Rig_T *body;
2066 emxArray_char_T_cartesian_tra_T *switch_expression;
2067 boolean_T b_bool;
2068 int32_T loop_ub;
2069 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
2070 };
2071
2072 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
2073
2074 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
2075
2076 int32_T exitg1;
2077 localB->n = obj->NumBodies;
2078 for (localB->b_kstr = 0; localB->b_kstr < 16; localB->b_kstr++) {
2079 localB->c_f1[localB->b_kstr] = tmp[localB->b_kstr];
2080 }
2081
2082 localB->b_kstr = Ttree->size[0] * Ttree->size[1];
2083 Ttree->size[0] = 1;
2084 localB->e = static_cast<int32_T>(localB->n);
2085 Ttree->size[1] = localB->e;
2086 c_emxEnsureCapacity_f_cell_wrap(Ttree, localB->b_kstr);
2087 if (localB->e != 0) {
2088 localB->ntilecols = localB->e - 1;
2089 if (0 <= localB->ntilecols) {
2090 memcpy(&localB->expl_temp.f1[0], &localB->c_f1[0], sizeof(real_T) << 4U);
2091 }
2092
2093 for (localB->b_jtilecol = 0; localB->b_jtilecol <= localB->ntilecols;
2094 localB->b_jtilecol++) {
2095 Ttree->data[localB->b_jtilecol] = localB->expl_temp;
2096 }
2097 }
2098
2099 localB->k = 1.0;
2100 localB->ntilecols = static_cast<int32_T>(localB->n) - 1;
2101 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
2102 if (0 <= localB->ntilecols) {
2103 for (localB->b_kstr = 0; localB->b_kstr < 5; localB->b_kstr++) {
2104 localB->b_dy[localB->b_kstr] = tmp_0[localB->b_kstr];
2105 }
2106 }
2107
2108 for (localB->b_jtilecol = 0; localB->b_jtilecol <= localB->ntilecols;
2109 localB->b_jtilecol++) {
2110 body = obj->Bodies[localB->b_jtilecol];
2111 localB->n = body->JointInternal.PositionNumber;
2112 localB->n += localB->k;
2113 if (localB->k > localB->n - 1.0) {
2114 localB->e = 0;
2115 localB->d = 0;
2116 } else {
2117 localB->e = static_cast<int32_T>(localB->k) - 1;
2118 localB->d = static_cast<int32_T>(localB->n - 1.0);
2119 }
2120
2121 localB->b_kstr = switch_expression->size[0] * switch_expression->size[1];
2122 switch_expression->size[0] = 1;
2123 switch_expression->size[1] = body->JointInternal.Type->size[1];
2124 cartes_emxEnsureCapacity_char_T(switch_expression, localB->b_kstr);
2125 loop_ub = body->JointInternal.Type->size[0] * body->JointInternal.Type->
2126 size[1] - 1;
2127 for (localB->b_kstr = 0; localB->b_kstr <= loop_ub; localB->b_kstr++) {
2128 switch_expression->data[localB->b_kstr] = body->JointInternal.Type->
2129 data[localB->b_kstr];
2130 }
2131
2132 b_bool = false;
2133 if (switch_expression->size[1] == 5) {
2134 localB->b_kstr = 1;
2135 do {
2136 exitg1 = 0;
2137 if (localB->b_kstr - 1 < 5) {
2138 loop_ub = localB->b_kstr - 1;
2139 if (switch_expression->data[loop_ub] != localB->b_dy[loop_ub]) {
2140 exitg1 = 1;
2141 } else {
2142 localB->b_kstr++;
2143 }
2144 } else {
2145 b_bool = true;
2146 exitg1 = 1;
2147 }
2148 } while (exitg1 == 0);
2149 }
2150
2151 if (b_bool) {
2152 localB->b_kstr = 0;
2153 } else {
2154 for (localB->b_kstr = 0; localB->b_kstr < 8; localB->b_kstr++) {
2155 localB->b_g[localB->b_kstr] = tmp_1[localB->b_kstr];
2156 }
2157
2158 b_bool = false;
2159 if (switch_expression->size[1] == 8) {
2160 localB->b_kstr = 1;
2161 do {
2162 exitg1 = 0;
2163 if (localB->b_kstr - 1 < 8) {
2164 loop_ub = localB->b_kstr - 1;
2165 if (switch_expression->data[loop_ub] != localB->b_g[loop_ub]) {
2166 exitg1 = 1;
2167 } else {
2168 localB->b_kstr++;
2169 }
2170 } else {
2171 b_bool = true;
2172 exitg1 = 1;
2173 }
2174 } while (exitg1 == 0);
2175 }
2176
2177 if (b_bool) {
2178 localB->b_kstr = 1;
2179 } else {
2180 localB->b_kstr = -1;
2181 }
2182 }
2183
2184 switch (localB->b_kstr) {
2185 case 0:
2186 memset(&localB->c_f1[0], 0, sizeof(real_T) << 4U);
2187 localB->c_f1[0] = 1.0;
2188 localB->c_f1[5] = 1.0;
2189 localB->c_f1[10] = 1.0;
2190 localB->c_f1[15] = 1.0;
2191 break;
2192
2193 case 1:
2194 ca_rigidBodyJoint_get_JointAxis(&body->JointInternal, localB->v, localB);
2195 localB->d -= localB->e;
2196 for (localB->b_kstr = 0; localB->b_kstr < localB->d; localB->b_kstr++) {
2197 localB->e_data[localB->b_kstr] = localB->e + localB->b_kstr;
2198 }
2199
2200 localB->result_data[0] = localB->v[0];
2201 localB->result_data[1] = localB->v[1];
2202 localB->result_data[2] = localB->v[2];
2203 if (0 <= (localB->d != 0) - 1) {
2204 localB->result_data[3] = qvec[localB->e_data[0]];
2205 }
2206
2207 localB->k = 1.0 / sqrt((localB->result_data[0] * localB->result_data[0] +
2208 localB->result_data[1] * localB->result_data[1]) + localB->result_data[2]
2209 * localB->result_data[2]);
2210 localB->v[0] = localB->result_data[0] * localB->k;
2211 localB->v[1] = localB->result_data[1] * localB->k;
2212 localB->v[2] = localB->result_data[2] * localB->k;
2213 localB->k = cos(localB->result_data[3]);
2214 localB->sth = sin(localB->result_data[3]);
2215 localB->tempR[0] = localB->v[0] * localB->v[0] * (1.0 - localB->k) +
2216 localB->k;
2217 localB->tempR_tmp = localB->v[1] * localB->v[0] * (1.0 - localB->k);
2218 localB->tempR_tmp_g = localB->v[2] * localB->sth;
2219 localB->tempR[1] = localB->tempR_tmp - localB->tempR_tmp_g;
2220 localB->tempR_tmp_l = localB->v[2] * localB->v[0] * (1.0 - localB->k);
2221 localB->tempR_tmp_d = localB->v[1] * localB->sth;
2222 localB->tempR[2] = localB->tempR_tmp_l + localB->tempR_tmp_d;
2223 localB->tempR[3] = localB->tempR_tmp + localB->tempR_tmp_g;
2224 localB->tempR[4] = localB->v[1] * localB->v[1] * (1.0 - localB->k) +
2225 localB->k;
2226 localB->tempR_tmp = localB->v[2] * localB->v[1] * (1.0 - localB->k);
2227 localB->tempR_tmp_g = localB->v[0] * localB->sth;
2228 localB->tempR[5] = localB->tempR_tmp - localB->tempR_tmp_g;
2229 localB->tempR[6] = localB->tempR_tmp_l - localB->tempR_tmp_d;
2230 localB->tempR[7] = localB->tempR_tmp + localB->tempR_tmp_g;
2231 localB->tempR[8] = localB->v[2] * localB->v[2] * (1.0 - localB->k) +
2232 localB->k;
2233 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
2234 localB->e = localB->b_kstr + 1;
2235 localB->R[localB->e - 1] = localB->tempR[(localB->e - 1) * 3];
2236 localB->e = localB->b_kstr + 1;
2237 localB->R[localB->e + 2] = localB->tempR[(localB->e - 1) * 3 + 1];
2238 localB->e = localB->b_kstr + 1;
2239 localB->R[localB->e + 5] = localB->tempR[(localB->e - 1) * 3 + 2];
2240 }
2241
2242 memset(&localB->c_f1[0], 0, sizeof(real_T) << 4U);
2243 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
2244 localB->d = localB->b_kstr << 2;
2245 localB->c_f1[localB->d] = localB->R[3 * localB->b_kstr];
2246 localB->c_f1[localB->d + 1] = localB->R[3 * localB->b_kstr + 1];
2247 localB->c_f1[localB->d + 2] = localB->R[3 * localB->b_kstr + 2];
2248 }
2249
2250 localB->c_f1[15] = 1.0;
2251 break;
2252
2253 default:
2254 ca_rigidBodyJoint_get_JointAxis(&body->JointInternal, localB->v, localB);
2255 memset(&localB->tempR[0], 0, 9U * sizeof(real_T));
2256 localB->tempR[0] = 1.0;
2257 localB->tempR[4] = 1.0;
2258 localB->tempR[8] = 1.0;
2259 for (localB->b_kstr = 0; localB->b_kstr < 3; localB->b_kstr++) {
2260 localB->d = localB->b_kstr << 2;
2261 localB->c_f1[localB->d] = localB->tempR[3 * localB->b_kstr];
2262 localB->c_f1[localB->d + 1] = localB->tempR[3 * localB->b_kstr + 1];
2263 localB->c_f1[localB->d + 2] = localB->tempR[3 * localB->b_kstr + 2];
2264 localB->c_f1[localB->b_kstr + 12] = localB->v[localB->b_kstr] *
2265 qvec[localB->e];
2266 }
2267
2268 localB->c_f1[3] = 0.0;
2269 localB->c_f1[7] = 0.0;
2270 localB->c_f1[11] = 0.0;
2271 localB->c_f1[15] = 1.0;
2272 break;
2273 }
2274
2275 for (localB->b_kstr = 0; localB->b_kstr < 16; localB->b_kstr++) {
2276 localB->a[localB->b_kstr] = body->
2277 JointInternal.JointToParentTransform[localB->b_kstr];
2278 }
2279
2280 for (localB->b_kstr = 0; localB->b_kstr < 16; localB->b_kstr++) {
2281 localB->b[localB->b_kstr] = body->
2282 JointInternal.ChildToJointTransform[localB->b_kstr];
2283 }
2284
2285 for (localB->b_kstr = 0; localB->b_kstr < 4; localB->b_kstr++) {
2286 for (localB->e = 0; localB->e < 4; localB->e++) {
2287 localB->d = localB->e << 2;
2288 loop_ub = localB->b_kstr + localB->d;
2289 localB->a_m[loop_ub] = 0.0;
2290 localB->a_m[loop_ub] += localB->c_f1[localB->d] * localB->a
2291 [localB->b_kstr];
2292 localB->a_m[loop_ub] += localB->c_f1[localB->d + 1] * localB->a
2293 [localB->b_kstr + 4];
2294 localB->a_m[loop_ub] += localB->c_f1[localB->d + 2] * localB->a
2295 [localB->b_kstr + 8];
2296 localB->a_m[loop_ub] += localB->c_f1[localB->d + 3] * localB->a
2297 [localB->b_kstr + 12];
2298 }
2299
2300 for (localB->e = 0; localB->e < 4; localB->e++) {
2301 localB->d = localB->e << 2;
2302 loop_ub = localB->b_kstr + localB->d;
2303 Ttree->data[localB->b_jtilecol].f1[loop_ub] = 0.0;
2304 Ttree->data[localB->b_jtilecol].f1[loop_ub] += localB->b[localB->d] *
2305 localB->a_m[localB->b_kstr];
2306 Ttree->data[localB->b_jtilecol].f1[loop_ub] += localB->b[localB->d + 1] *
2307 localB->a_m[localB->b_kstr + 4];
2308 Ttree->data[localB->b_jtilecol].f1[loop_ub] += localB->b[localB->d + 2] *
2309 localB->a_m[localB->b_kstr + 8];
2310 Ttree->data[localB->b_jtilecol].f1[loop_ub] += localB->b[localB->d + 3] *
2311 localB->a_m[localB->b_kstr + 12];
2312 }
2313 }
2314
2315 localB->k = localB->n;
2316 if (body->ParentIndex > 0.0) {
2317 for (localB->b_kstr = 0; localB->b_kstr < 16; localB->b_kstr++) {
2318 localB->a[localB->b_kstr] = Ttree->data[static_cast<int32_T>
2319 (body->ParentIndex) - 1].f1[localB->b_kstr];
2320 }
2321
2322 for (localB->b_kstr = 0; localB->b_kstr < 4; localB->b_kstr++) {
2323 for (localB->e = 0; localB->e < 4; localB->e++) {
2324 localB->d = localB->e << 2;
2325 loop_ub = localB->b_kstr + localB->d;
2326 localB->a_m[loop_ub] = 0.0;
2327 localB->a_m[loop_ub] += Ttree->data[localB->b_jtilecol].f1[localB->d] *
2328 localB->a[localB->b_kstr];
2329 localB->a_m[loop_ub] += Ttree->data[localB->b_jtilecol].f1[localB->d +
2330 1] * localB->a[localB->b_kstr + 4];
2331 localB->a_m[loop_ub] += Ttree->data[localB->b_jtilecol].f1[localB->d +
2332 2] * localB->a[localB->b_kstr + 8];
2333 localB->a_m[loop_ub] += Ttree->data[localB->b_jtilecol].f1[localB->d +
2334 3] * localB->a[localB->b_kstr + 12];
2335 }
2336 }
2337
2338 for (localB->b_kstr = 0; localB->b_kstr < 16; localB->b_kstr++) {
2339 Ttree->data[localB->b_jtilecol].f1[localB->b_kstr] = localB->a_m
2340 [localB->b_kstr];
2341 }
2342 }
2343 }
2344
2345 cartesian_trajec_emxFree_char_T(&switch_expression);
2346}
2347
2348static void cartesian_t_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
2349 **pEmxArray)
2350{
2351 if (*pEmxArray != (emxArray_f_cell_wrap_cartesia_T *)NULL) {
2352 if (((*pEmxArray)->data != (f_cell_wrap_cartesian_traject_T *)NULL) &&
2353 (*pEmxArray)->canFreeData) {
2354 free((*pEmxArray)->data);
2355 }
2356
2357 free((*pEmxArray)->size);
2358 free(*pEmxArray);
2359 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)NULL;
2360 }
2361}
2362
2363static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_tr_T
2364 *pStruct)
2365{
2366 cartesian_trajec_emxFree_char_T(&pStruct->Type);
2367}
2368
2369static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
2370 *pStruct)
2371{
2372 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
2373 emxFreeStruct_c_rigidBodyJoint(&pStruct->JointInternal);
2374}
2375
2376static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
2377 *pStruct)
2378{
2379 emxFreeStruct_o_robotics_manip_(&pStruct->Base);
2380}
2381
2382static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
2383 *pStruct)
2384{
2385 emxFreeStruct_p_robotics_manip_(&pStruct->TreeInternal);
2386}
2387
2388static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
2389 *pStruct)
2390{
2391 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
2392 emxFreeStruct_c_rigidBodyJoint(&pStruct->JointInternal);
2393}
2394
2395//
2396// System initialize for atomic system:
2397// synthesized block
2398// synthesized block
2399//
2400void cartesian_tra_MATLABSystem_Init(B_MATLABSystem_cartesian_traj_T *localB,
2401 DW_MATLABSystem_cartesian_tra_T *localDW)
2402{
2403 emxInitStruct_robotics_slmanip_(&localDW->obj);
2404 emxInitStruct_n_robotics_manip_(&localDW->gobj_1);
2405 emxInitStruct_n_robotics_manip_(&localDW->gobj_16);
2406 emxInitStruct_n_robotics_manip_(&localDW->gobj_15);
2407 emxInitStruct_n_robotics_manip_(&localDW->gobj_14);
2408 emxInitStruct_n_robotics_manip_(&localDW->gobj_13);
2409 emxInitStruct_n_robotics_manip_(&localDW->gobj_12);
2410 emxInitStruct_n_robotics_manip_(&localDW->gobj_11);
2411 emxInitStruct_n_robotics_manip_(&localDW->gobj_10);
2412 emxInitStruct_n_robotics_manip_(&localDW->gobj_9);
2413 emxInitStruct_n_robotics_manip_(&localDW->gobj_8);
2414 emxInitStruct_n_robotics_manip_(&localDW->gobj_7);
2415 emxInitStruct_n_robotics_manip_(&localDW->gobj_6);
2416 emxInitStruct_n_robotics_manip_(&localDW->gobj_5);
2417 emxInitStruct_n_robotics_manip_(&localDW->gobj_4);
2418 emxInitStruct_n_robotics_manip_(&localDW->gobj_3);
2419 emxInitStruct_n_robotics_manip_(&localDW->gobj_2);
2420
2421 // Start for MATLABSystem: '<S4>/MATLAB System'
2422 localDW->obj.isInitialized = 0;
2423 localDW->objisempty = true;
2424 localDW->obj.isInitialized = 1;
2425 car_RigidBodyTree_RigidBodyTree(&localDW->obj.TreeInternal, &localDW->gobj_2,
2426 &localDW->gobj_4, &localDW->gobj_5, &localDW->gobj_6, &localDW->gobj_7,
2427 &localDW->gobj_8, &localDW->gobj_9, &localDW->gobj_3, localB);
2428}
2429
2430//
2431// Output and update for atomic system:
2432// synthesized block
2433// synthesized block
2434//
2435void cartesian_trajecto_MATLABSystem(const real_T rtu_0[6],
2436 B_MATLABSystem_cartesian_traj_T *localB, DW_MATLABSystem_cartesian_tra_T
2437 *localDW)
2438{
2439 emxArray_f_cell_wrap_cartesia_T *obj;
2440
2441 // MATLABSystem: '<S4>/MATLAB System'
2442 for (localB->i = 0; localB->i < 6; localB->i++) {
2443 localB->u0[localB->i] = rtu_0[localB->i];
2444 }
2445
2446 cartesian_t_emxInit_f_cell_wrap(&obj, 2);
2447
2448 // MATLABSystem: '<S4>/MATLAB System'
2449 RigidBodyTree_forwardKinematics(&localDW->obj.TreeInternal, localB->u0, obj,
2450 localB);
2451 cartesian_t_emxFree_f_cell_wrap(&obj);
2452}
2453
2454//
2455// Termination for atomic system:
2456// synthesized block
2457// synthesized block
2458//
2459void cartesian_tra_MATLABSystem_Term(DW_MATLABSystem_cartesian_tra_T *localDW)
2460{
2461 emxFreeStruct_robotics_slmanip_(&localDW->obj);
2462 emxFreeStruct_n_robotics_manip_(&localDW->gobj_1);
2463 emxFreeStruct_n_robotics_manip_(&localDW->gobj_16);
2464 emxFreeStruct_n_robotics_manip_(&localDW->gobj_15);
2465 emxFreeStruct_n_robotics_manip_(&localDW->gobj_14);
2466 emxFreeStruct_n_robotics_manip_(&localDW->gobj_13);
2467 emxFreeStruct_n_robotics_manip_(&localDW->gobj_12);
2468 emxFreeStruct_n_robotics_manip_(&localDW->gobj_11);
2469 emxFreeStruct_n_robotics_manip_(&localDW->gobj_10);
2470 emxFreeStruct_n_robotics_manip_(&localDW->gobj_9);
2471 emxFreeStruct_n_robotics_manip_(&localDW->gobj_8);
2472 emxFreeStruct_n_robotics_manip_(&localDW->gobj_7);
2473 emxFreeStruct_n_robotics_manip_(&localDW->gobj_6);
2474 emxFreeStruct_n_robotics_manip_(&localDW->gobj_5);
2475 emxFreeStruct_n_robotics_manip_(&localDW->gobj_4);
2476 emxFreeStruct_n_robotics_manip_(&localDW->gobj_3);
2477 emxFreeStruct_n_robotics_manip_(&localDW->gobj_2);
2478}
2479
2480// Function for MATLAB Function: '<Root>/Traslazione '
2481static real_T cartesian_trajectory_pla_norm_l(const real_T x[3])
2482{
2483 real_T y;
2484 real_T scale;
2485 real_T absxk;
2486 real_T t;
2487 scale = 3.3121686421112381E-170;
2488 absxk = fabs(x[0]);
2489 if (absxk > 3.3121686421112381E-170) {
2490 y = 1.0;
2491 scale = absxk;
2492 } else {
2493 t = absxk / 3.3121686421112381E-170;
2494 y = t * t;
2495 }
2496
2497 absxk = fabs(x[1]);
2498 if (absxk > scale) {
2499 t = scale / absxk;
2500 y = y * t * t + 1.0;
2501 scale = absxk;
2502 } else {
2503 t = absxk / scale;
2504 y += t * t;
2505 }
2506
2507 absxk = fabs(x[2]);
2508 if (absxk > scale) {
2509 t = scale / absxk;
2510 y = y * t * t + 1.0;
2511 scale = absxk;
2512 } else {
2513 t = absxk / scale;
2514 y += t * t;
2515 }
2516
2517 return scale * sqrt(y);
2518}
2519
2520static void cartesian_trajec_emxInit_real_T(emxArray_real_T_cartesian_tra_T
2521 **pEmxArray, int32_T numDimensions)
2522{
2523 emxArray_real_T_cartesian_tra_T *emxArray;
2524 *pEmxArray = (emxArray_real_T_cartesian_tra_T *)malloc(sizeof
2525 (emxArray_real_T_cartesian_tra_T));
2526 emxArray = *pEmxArray;
2527 emxArray->data = (real_T *)NULL;
2528 emxArray->numDimensions = numDimensions;
2529 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
2530 emxArray->allocatedSize = 0;
2531 emxArray->canFreeData = true;
2532 for (cartesian_trajectory_planner_B.i_p = 0;
2533 cartesian_trajectory_planner_B.i_p < numDimensions;
2534 cartesian_trajectory_planner_B.i_p++) {
2535 emxArray->size[cartesian_trajectory_planner_B.i_p] = 0;
2536 }
2537}
2538
2539static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_tra_T
2540 *emxArray, int32_T oldNumel)
2541{
2542 void *newData;
2543 if (oldNumel < 0) {
2544 oldNumel = 0;
2545 }
2546
2547 cartesian_trajectory_planner_B.newNumel_f = 1;
2548 for (cartesian_trajectory_planner_B.i_gd = 0;
2549 cartesian_trajectory_planner_B.i_gd < emxArray->numDimensions;
2550 cartesian_trajectory_planner_B.i_gd++) {
2551 cartesian_trajectory_planner_B.newNumel_f *= emxArray->
2552 size[cartesian_trajectory_planner_B.i_gd];
2553 }
2554
2555 if (cartesian_trajectory_planner_B.newNumel_f > emxArray->allocatedSize) {
2556 cartesian_trajectory_planner_B.i_gd = emxArray->allocatedSize;
2557 if (cartesian_trajectory_planner_B.i_gd < 16) {
2558 cartesian_trajectory_planner_B.i_gd = 16;
2559 }
2560
2561 while (cartesian_trajectory_planner_B.i_gd <
2562 cartesian_trajectory_planner_B.newNumel_f) {
2563 if (cartesian_trajectory_planner_B.i_gd > 1073741823) {
2564 cartesian_trajectory_planner_B.i_gd = MAX_int32_T;
2565 } else {
2566 cartesian_trajectory_planner_B.i_gd <<= 1;
2567 }
2568 }
2569
2570 newData = calloc(static_cast<uint32_T>(cartesian_trajectory_planner_B.i_gd),
2571 sizeof(real_T));
2572 if (emxArray->data != NULL) {
2573 memcpy(newData, emxArray->data, sizeof(real_T) * oldNumel);
2574 if (emxArray->canFreeData) {
2575 free(emxArray->data);
2576 }
2577 }
2578
2579 emxArray->data = (real_T *)newData;
2580 emxArray->allocatedSize = cartesian_trajectory_planner_B.i_gd;
2581 emxArray->canFreeData = true;
2582 }
2583}
2584
2585static void cartesian_traj_emxInit_char_T_a(emxArray_char_T_cartesian_tra_T
2586 **pEmxArray, int32_T numDimensions)
2587{
2588 emxArray_char_T_cartesian_tra_T *emxArray;
2589 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)malloc(sizeof
2590 (emxArray_char_T_cartesian_tra_T));
2591 emxArray = *pEmxArray;
2592 emxArray->data = (char_T *)NULL;
2593 emxArray->numDimensions = numDimensions;
2594 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
2595 emxArray->allocatedSize = 0;
2596 emxArray->canFreeData = true;
2597 for (cartesian_trajectory_planner_B.i_g = 0;
2598 cartesian_trajectory_planner_B.i_g < numDimensions;
2599 cartesian_trajectory_planner_B.i_g++) {
2600 emxArray->size[cartesian_trajectory_planner_B.i_g] = 0;
2601 }
2602}
2603
2604static void cart_emxEnsureCapacity_char_T_a(emxArray_char_T_cartesian_tra_T
2605 *emxArray, int32_T oldNumel)
2606{
2607 void *newData;
2608 if (oldNumel < 0) {
2609 oldNumel = 0;
2610 }
2611
2612 cartesian_trajectory_planner_B.newNumel = 1;
2613 for (cartesian_trajectory_planner_B.i_ho = 0;
2614 cartesian_trajectory_planner_B.i_ho < emxArray->numDimensions;
2615 cartesian_trajectory_planner_B.i_ho++) {
2616 cartesian_trajectory_planner_B.newNumel *= emxArray->
2617 size[cartesian_trajectory_planner_B.i_ho];
2618 }
2619
2620 if (cartesian_trajectory_planner_B.newNumel > emxArray->allocatedSize) {
2621 cartesian_trajectory_planner_B.i_ho = emxArray->allocatedSize;
2622 if (cartesian_trajectory_planner_B.i_ho < 16) {
2623 cartesian_trajectory_planner_B.i_ho = 16;
2624 }
2625
2626 while (cartesian_trajectory_planner_B.i_ho <
2627 cartesian_trajectory_planner_B.newNumel) {
2628 if (cartesian_trajectory_planner_B.i_ho > 1073741823) {
2629 cartesian_trajectory_planner_B.i_ho = MAX_int32_T;
2630 } else {
2631 cartesian_trajectory_planner_B.i_ho <<= 1;
2632 }
2633 }
2634
2635 newData = calloc(static_cast<uint32_T>(cartesian_trajectory_planner_B.i_ho),
2636 sizeof(char_T));
2637 if (emxArray->data != NULL) {
2638 memcpy(newData, emxArray->data, sizeof(char_T) * oldNumel);
2639 if (emxArray->canFreeData) {
2640 free(emxArray->data);
2641 }
2642 }
2643
2644 emxArray->data = (char_T *)newData;
2645 emxArray->allocatedSize = cartesian_trajectory_planner_B.i_ho;
2646 emxArray->canFreeData = true;
2647 }
2648}
2649
2650static void cartesian_trajec_emxFree_real_T(emxArray_real_T_cartesian_tra_T
2651 **pEmxArray)
2652{
2653 if (*pEmxArray != (emxArray_real_T_cartesian_tra_T *)NULL) {
2654 if (((*pEmxArray)->data != (real_T *)NULL) && (*pEmxArray)->canFreeData) {
2655 free((*pEmxArray)->data);
2656 }
2657
2658 free((*pEmxArray)->size);
2659 free(*pEmxArray);
2660 *pEmxArray = (emxArray_real_T_cartesian_tra_T *)NULL;
2661 }
2662}
2663
2664static void cartesian_traj_emxFree_char_T_a(emxArray_char_T_cartesian_tra_T
2665 **pEmxArray)
2666{
2667 if (*pEmxArray != (emxArray_char_T_cartesian_tra_T *)NULL) {
2668 if (((*pEmxArray)->data != (char_T *)NULL) && (*pEmxArray)->canFreeData) {
2669 free((*pEmxArray)->data);
2670 }
2671
2672 free((*pEmxArray)->size);
2673 free(*pEmxArray);
2674 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)NULL;
2675 }
2676}
2677
2678static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
2679 f_robotics_manip_internal_IKE_T *iobj_0)
2680{
2681 real_T n;
2682 emxArray_real_T_cartesian_tra_T *A;
2683 emxArray_real_T_cartesian_tra_T *b;
2684 real_T m;
2685 c_rigidBodyJoint_cartesian_as_T *joint;
2686 real_T pnum;
2687 int32_T d;
2688 int32_T b_i;
2689 emxArray_real_T_cartesian_tra_T *e;
2690 int32_T j;
2691 int32_T p;
2692 emxArray_real_T_cartesian_tra_T *s;
2693 x_robotics_manip_internal_Rig_T *obj_0;
2694 w_robotics_manip_internal_Rig_T *body;
2695 int32_T c;
2696 boolean_T b_bool;
2697 emxArray_char_T_cartesian_tra_T *a;
2698 int8_T b_I[16];
2699 int32_T m_0;
2700 real_T t;
2701 int32_T b_kstr;
2702 char_T b_0[5];
2703 real_T pnum_0;
2704 int32_T loop_ub;
2705 real_T tmp;
2706 real_T w;
2707 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
2708
2709 int32_T exitg1;
2710 cartesian_trajec_emxInit_real_T(&A, 2);
2711 n = obj->RigidBodyTreeInternal->PositionNumber;
2712 b_kstr = A->size[0] * A->size[1];
2713 c = static_cast<int32_T>(n);
2714 A->size[0] = c;
2715 loop_ub = static_cast<int32_T>(2.0 * n);
2716 A->size[1] = loop_ub;
2717 cartes_emxEnsureCapacity_real_T(A, b_kstr);
2718 m_0 = loop_ub * c - 1;
2719 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
2720 A->data[b_kstr] = 0.0;
2721 }
2722
2723 cartesian_trajec_emxInit_real_T(&b, 1);
2724 b_kstr = b->size[0];
2725 b->size[0] = loop_ub;
2726 cartes_emxEnsureCapacity_real_T(b, b_kstr);
2727 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
2728 b->data[b_kstr] = 0.0;
2729 }
2730
2731 n = 1.0;
2732 m = 1.0;
2733 pnum = obj->RigidBodyTreeInternal->NumBodies;
2734 d = static_cast<int32_T>(pnum) - 1;
2735 cartesian_trajec_emxInit_real_T(&e, 2);
2736 cartesian_trajec_emxInit_real_T(&s, 2);
2737 cartesian_traj_emxInit_char_T_a(&a, 2);
2738 if (0 <= d) {
2739 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
2740 b_0[b_kstr] = tmp_0[b_kstr];
2741 }
2742 }
2743
2744 for (b_i = 0; b_i <= d; b_i++) {
2745 body = obj->RigidBodyTreeInternal->Bodies[b_i];
2746 joint = body->JointInternal;
2747 pnum = joint->PositionNumber;
2748 b_kstr = a->size[0] * a->size[1];
2749 a->size[0] = 1;
2750 a->size[1] = joint->Type->size[1];
2751 cart_emxEnsureCapacity_char_T_a(a, b_kstr);
2752 loop_ub = joint->Type->size[0] * joint->Type->size[1] - 1;
2753 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2754 a->data[b_kstr] = joint->Type->data[b_kstr];
2755 }
2756
2757 b_bool = false;
2758 if (a->size[1] == 5) {
2759 b_kstr = 1;
2760 do {
2761 exitg1 = 0;
2762 if (b_kstr - 1 < 5) {
2763 loop_ub = b_kstr - 1;
2764 if (a->data[loop_ub] != b_0[loop_ub]) {
2765 exitg1 = 1;
2766 } else {
2767 b_kstr++;
2768 }
2769 } else {
2770 b_bool = true;
2771 exitg1 = 1;
2772 }
2773 } while (exitg1 == 0);
2774 }
2775
2776 if (!b_bool) {
2777 tmp = (n + pnum) - 1.0;
2778 if (n > tmp) {
2779 j = 0;
2780 } else {
2781 j = static_cast<int32_T>(n) - 1;
2782 }
2783
2784 w = m + pnum;
2785 if (m > w - 1.0) {
2786 p = 0;
2787 } else {
2788 p = static_cast<int32_T>(m) - 1;
2789 }
2790
2791 if (pnum < 0.0) {
2792 t = 0.0;
2793 pnum_0 = 0.0;
2794 } else {
2795 t = pnum;
2796 pnum_0 = pnum;
2797 }
2798
2799 m_0 = static_cast<int32_T>(pnum_0) - 1;
2800 b_kstr = s->size[0] * s->size[1];
2801 c = static_cast<int32_T>(t);
2802 s->size[0] = c;
2803 s->size[1] = c;
2804 cartes_emxEnsureCapacity_real_T(s, b_kstr);
2805 loop_ub = c * c - 1;
2806 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2807 s->data[b_kstr] = 0.0;
2808 }
2809
2810 if (c > 0) {
2811 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
2812 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
2813 }
2814 }
2815
2816 loop_ub = s->size[1];
2817 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
2818 m_0 = s->size[0];
2819 for (c = 0; c < m_0; c++) {
2820 A->data[(j + c) + A->size[0] * (p + b_kstr)] = s->data[s->size[0] *
2821 b_kstr + c];
2822 }
2823 }
2824
2825 if (n > tmp) {
2826 j = 0;
2827 } else {
2828 j = static_cast<int32_T>(n) - 1;
2829 }
2830
2831 tmp = 2.0 * pnum + m;
2832 if (w > tmp - 1.0) {
2833 p = 0;
2834 } else {
2835 p = static_cast<int32_T>(w) - 1;
2836 }
2837
2838 if (pnum < 0.0) {
2839 t = 0.0;
2840 pnum_0 = 0.0;
2841 } else {
2842 t = pnum;
2843 pnum_0 = pnum;
2844 }
2845
2846 m_0 = static_cast<int32_T>(pnum_0) - 1;
2847 b_kstr = s->size[0] * s->size[1];
2848 c = static_cast<int32_T>(t);
2849 s->size[0] = c;
2850 s->size[1] = c;
2851 cartes_emxEnsureCapacity_real_T(s, b_kstr);
2852 loop_ub = c * c - 1;
2853 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2854 s->data[b_kstr] = 0.0;
2855 }
2856
2857 if (c > 0) {
2858 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
2859 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
2860 }
2861 }
2862
2863 loop_ub = s->size[1];
2864 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
2865 m_0 = s->size[0];
2866 for (c = 0; c < m_0; c++) {
2867 A->data[(j + c) + A->size[0] * (p + b_kstr)] = -s->data[s->size[0] *
2868 b_kstr + c];
2869 }
2870 }
2871
2872 b_kstr = e->size[0] * e->size[1];
2873 e->size[0] = joint->PositionLimitsInternal->size[0];
2874 e->size[1] = 2;
2875 cartes_emxEnsureCapacity_real_T(e, b_kstr);
2876 loop_ub = joint->PositionLimitsInternal->size[0] *
2877 joint->PositionLimitsInternal->size[1] - 1;
2878 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2879 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
2880 }
2881
2882 b->data[static_cast<int32_T>(m) - 1] = e->data[1];
2883 b_kstr = e->size[0] * e->size[1];
2884 e->size[0] = joint->PositionLimitsInternal->size[0];
2885 e->size[1] = 2;
2886 cartes_emxEnsureCapacity_real_T(e, b_kstr);
2887 loop_ub = joint->PositionLimitsInternal->size[0] *
2888 joint->PositionLimitsInternal->size[1] - 1;
2889 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2890 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
2891 }
2892
2893 b->data[static_cast<int32_T>(m + 1.0) - 1] = -e->data[0];
2894 m = tmp;
2895 }
2896
2897 n += pnum;
2898 }
2899
2900 cartesian_trajec_emxFree_real_T(&s);
2901 b_kstr = A->size[0] * A->size[1];
2902 c = obj->Solver->ConstraintMatrix->size[0] * obj->Solver->
2903 ConstraintMatrix->size[1];
2904 obj->Solver->ConstraintMatrix->size[0] = A->size[0];
2905 obj->Solver->ConstraintMatrix->size[1] = A->size[1];
2906 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintMatrix, c);
2907 loop_ub = b_kstr - 1;
2908 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2909 obj->Solver->ConstraintMatrix->data[b_kstr] = A->data[b_kstr];
2910 }
2911
2912 cartesian_trajec_emxFree_real_T(&A);
2913 b_kstr = obj->Solver->ConstraintBound->size[0];
2914 obj->Solver->ConstraintBound->size[0] = b->size[0];
2915 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintBound, b_kstr);
2916 loop_ub = b->size[0];
2917 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
2918 obj->Solver->ConstraintBound->data[b_kstr] = b->data[b_kstr];
2919 }
2920
2921 obj_0 = obj->RigidBodyTreeInternal;
2922 b_kstr = e->size[0] * e->size[1];
2923 e->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
2924 e->size[1] = 2;
2925 cartes_emxEnsureCapacity_real_T(e, b_kstr);
2926 loop_ub = (static_cast<int32_T>(obj_0->PositionNumber) << 1) - 1;
2927 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2928 e->data[b_kstr] = 0.0;
2929 }
2930
2931 n = 1.0;
2932 m = obj_0->NumBodies;
2933 c = static_cast<int32_T>(m) - 1;
2934 if (0 <= c) {
2935 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
2936 b_0[b_kstr] = tmp_0[b_kstr];
2937 }
2938 }
2939
2940 for (b_i = 0; b_i <= c; b_i++) {
2941 body = obj_0->Bodies[b_i];
2942 b_kstr = a->size[0] * a->size[1];
2943 a->size[0] = 1;
2944 a->size[1] = body->JointInternal->Type->size[1];
2945 cart_emxEnsureCapacity_char_T_a(a, b_kstr);
2946 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
2947 Type->size[1] - 1;
2948 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
2949 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
2950 }
2951
2952 b_bool = false;
2953 if (a->size[1] == 5) {
2954 b_kstr = 1;
2955 do {
2956 exitg1 = 0;
2957 if (b_kstr - 1 < 5) {
2958 loop_ub = b_kstr - 1;
2959 if (a->data[loop_ub] != b_0[loop_ub]) {
2960 exitg1 = 1;
2961 } else {
2962 b_kstr++;
2963 }
2964 } else {
2965 b_bool = true;
2966 exitg1 = 1;
2967 }
2968 } while (exitg1 == 0);
2969 }
2970
2971 if (!b_bool) {
2972 pnum = body->JointInternal->PositionNumber;
2973 tmp = n + pnum;
2974 if (n > tmp - 1.0) {
2975 d = 0;
2976 } else {
2977 d = static_cast<int32_T>(n) - 1;
2978 }
2979
2980 joint = body->JointInternal;
2981 loop_ub = joint->PositionLimitsInternal->size[0];
2982 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
2983 e->data[d + b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
2984 }
2985
2986 loop_ub = joint->PositionLimitsInternal->size[0];
2987 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
2988 e->data[(d + b_kstr) + e->size[0]] = joint->PositionLimitsInternal->
2989 data[b_kstr + joint->PositionLimitsInternal->size[0]];
2990 }
2991
2992 n = tmp;
2993 }
2994 }
2995
2996 cartesian_traj_emxFree_char_T_a(&a);
2997 b_kstr = obj->Limits->size[0] * obj->Limits->size[1];
2998 obj->Limits->size[0] = e->size[0];
2999 obj->Limits->size[1] = 2;
3000 cartes_emxEnsureCapacity_real_T(obj->Limits, b_kstr);
3001 loop_ub = e->size[0] * e->size[1] - 1;
3002 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
3003 obj->Limits->data[b_kstr] = e->data[b_kstr];
3004 }
3005
3006 cartesian_trajec_emxFree_real_T(&e);
3007 obj->Solver->ExtraArgs = iobj_0;
3008 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
3009 obj->Solver->ExtraArgs->WeightMatrix[b_kstr] = 0.0;
3010 }
3011
3012 obj->Solver->ExtraArgs->Robot = obj->RigidBodyTreeInternal;
3013 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3014 b_I[b_kstr] = 0;
3015 }
3016
3017 b_I[0] = 1;
3018 b_I[5] = 1;
3019 b_I[10] = 1;
3020 b_I[15] = 1;
3021 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
3022 obj->Solver->ExtraArgs->Tform[b_kstr] = b_I[b_kstr];
3023 }
3024
3025 obj->Solver->ExtraArgs->BodyName->size[0] = 1;
3026 obj->Solver->ExtraArgs->BodyName->size[1] = 0;
3027 b_kstr = obj->Solver->ExtraArgs->ErrTemp->size[0];
3028 obj->Solver->ExtraArgs->ErrTemp->size[0] = 6;
3029 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->ErrTemp, b_kstr);
3030 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3031 obj->Solver->ExtraArgs->ErrTemp->data[b_kstr] = 0.0;
3032 }
3033
3034 obj->Solver->ExtraArgs->CostTemp = 0.0;
3035 b_kstr = b->size[0];
3036 b->size[0] = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
3037 cartes_emxEnsureCapacity_real_T(b, b_kstr);
3038 loop_ub = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
3039 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
3040 b->data[b_kstr] = 0.0;
3041 }
3042
3043 b_kstr = obj->Solver->ExtraArgs->GradTemp->size[0];
3044 obj->Solver->ExtraArgs->GradTemp->size[0] = b->size[0];
3045 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->GradTemp, b_kstr);
3046 loop_ub = b->size[0];
3047 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
3048 obj->Solver->ExtraArgs->GradTemp->data[b_kstr] = b->data[b_kstr];
3049 }
3050
3051 cartesian_trajec_emxFree_real_T(&b);
3052}
3053
3054static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
3055 const real_T tform[16], const real_T weights[6])
3056{
3057 real_T weightMatrix[36];
3058 f_robotics_manip_internal_IKE_T *args;
3059 int32_T b_j;
3060 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
3061 'e', 'e' };
3062
3063 memset(&weightMatrix[0], 0, 36U * sizeof(real_T));
3064 for (b_j = 0; b_j < 6; b_j++) {
3065 weightMatrix[b_j + 6 * b_j] = weights[b_j];
3066 }
3067
3068 args = obj->Solver->ExtraArgs;
3069 for (b_j = 0; b_j < 36; b_j++) {
3070 args->WeightMatrix[b_j] = weightMatrix[b_j];
3071 }
3072
3073 b_j = args->BodyName->size[0] * args->BodyName->size[1];
3074 args->BodyName->size[0] = 1;
3075 args->BodyName->size[1] = 11;
3076 cart_emxEnsureCapacity_char_T_a(args->BodyName, b_j);
3077 for (b_j = 0; b_j < 11; b_j++) {
3078 args->BodyName->data[b_j] = tmp[b_j];
3079 }
3080
3081 for (b_j = 0; b_j < 16; b_j++) {
3082 args->Tform[b_j] = tform[b_j];
3083 }
3084}
3085
3086static void RigidBodyTree_validateConfigu_a(x_robotics_manip_internal_Rig_T *obj,
3087 real_T Q[6])
3088{
3089 emxArray_real_T_cartesian_tra_T *limits;
3090 boolean_T ubOK[6];
3091 boolean_T lbOK[6];
3092 real_T k;
3093 w_robotics_manip_internal_Rig_T *body;
3094 real_T pnum;
3095 int32_T c;
3096 int32_T f;
3097 int32_T ii_data[6];
3098 boolean_T b_bool;
3099 emxArray_char_T_cartesian_tra_T *a;
3100 c_rigidBodyJoint_cartesian_as_T *obj_0;
3101 int32_T idx;
3102 int32_T b_kstr;
3103 char_T b[5];
3104 int32_T loop_ub;
3105 emxArray_real_T_cartesian_tra_T *limits_0;
3106 emxArray_real_T_cartesian_tra_T *limits_1;
3107 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
3108
3109 boolean_T guard1 = false;
3110 int32_T exitg1;
3111 boolean_T exitg2;
3112 cartesian_trajec_emxInit_real_T(&limits, 2);
3113 b_kstr = limits->size[0] * limits->size[1];
3114 limits->size[0] = static_cast<int32_T>(obj->PositionNumber);
3115 limits->size[1] = 2;
3116 cartes_emxEnsureCapacity_real_T(limits, b_kstr);
3117 loop_ub = (static_cast<int32_T>(obj->PositionNumber) << 1) - 1;
3118 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
3119 limits->data[b_kstr] = 0.0;
3120 }
3121
3122 k = 1.0;
3123 pnum = obj->NumBodies;
3124 c = static_cast<int32_T>(pnum) - 1;
3125 cartesian_traj_emxInit_char_T_a(&a, 2);
3126 if (0 <= c) {
3127 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
3128 b[b_kstr] = tmp[b_kstr];
3129 }
3130 }
3131
3132 for (idx = 0; idx <= c; idx++) {
3133 body = obj->Bodies[idx];
3134 b_kstr = a->size[0] * a->size[1];
3135 a->size[0] = 1;
3136 a->size[1] = body->JointInternal->Type->size[1];
3137 cart_emxEnsureCapacity_char_T_a(a, b_kstr);
3138 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
3139 Type->size[1] - 1;
3140 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
3141 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
3142 }
3143
3144 b_bool = false;
3145 if (a->size[1] == 5) {
3146 b_kstr = 1;
3147 do {
3148 exitg1 = 0;
3149 if (b_kstr - 1 < 5) {
3150 loop_ub = b_kstr - 1;
3151 if (a->data[loop_ub] != b[loop_ub]) {
3152 exitg1 = 1;
3153 } else {
3154 b_kstr++;
3155 }
3156 } else {
3157 b_bool = true;
3158 exitg1 = 1;
3159 }
3160 } while (exitg1 == 0);
3161 }
3162
3163 if (!b_bool) {
3164 pnum = body->JointInternal->PositionNumber;
3165 pnum += k;
3166 if (k > pnum - 1.0) {
3167 f = 0;
3168 } else {
3169 f = static_cast<int32_T>(k) - 1;
3170 }
3171
3172 obj_0 = body->JointInternal;
3173 loop_ub = obj_0->PositionLimitsInternal->size[0];
3174 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
3175 limits->data[f + b_kstr] = obj_0->PositionLimitsInternal->data[b_kstr];
3176 }
3177
3178 loop_ub = obj_0->PositionLimitsInternal->size[0];
3179 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
3180 limits->data[(f + b_kstr) + limits->size[0]] =
3181 obj_0->PositionLimitsInternal->data[b_kstr +
3182 obj_0->PositionLimitsInternal->size[0]];
3183 }
3184
3185 k = pnum;
3186 }
3187 }
3188
3189 cartesian_traj_emxFree_char_T_a(&a);
3190 cartesian_trajec_emxInit_real_T(&limits_0, 1);
3191 loop_ub = limits->size[0];
3192 b_kstr = limits_0->size[0];
3193 limits_0->size[0] = loop_ub;
3194 cartes_emxEnsureCapacity_real_T(limits_0, b_kstr);
3195 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
3196 limits_0->data[b_kstr] = limits->data[b_kstr + limits->size[0]] +
3197 4.4408920985006262E-16;
3198 }
3199
3200 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3201 ubOK[b_kstr] = (Q[b_kstr] <= limits_0->data[b_kstr]);
3202 }
3203
3204 cartesian_trajec_emxFree_real_T(&limits_0);
3205 cartesian_trajec_emxInit_real_T(&limits_1, 1);
3206 loop_ub = limits->size[0];
3207 b_kstr = limits_1->size[0];
3208 limits_1->size[0] = loop_ub;
3209 cartes_emxEnsureCapacity_real_T(limits_1, b_kstr);
3210 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
3211 limits_1->data[b_kstr] = limits->data[b_kstr] - 4.4408920985006262E-16;
3212 }
3213
3214 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
3215 lbOK[b_kstr] = (Q[b_kstr] >= limits_1->data[b_kstr]);
3216 }
3217
3218 cartesian_trajec_emxFree_real_T(&limits_1);
3219 b_bool = true;
3220 idx = 0;
3221 exitg2 = false;
3222 while ((!exitg2) && (idx < 6)) {
3223 if (!ubOK[idx]) {
3224 b_bool = false;
3225 exitg2 = true;
3226 } else {
3227 idx++;
3228 }
3229 }
3230
3231 guard1 = false;
3232 if (b_bool) {
3233 b_bool = true;
3234 idx = 0;
3235 exitg2 = false;
3236 while ((!exitg2) && (idx < 6)) {
3237 if (!lbOK[idx]) {
3238 b_bool = false;
3239 exitg2 = true;
3240 } else {
3241 idx++;
3242 }
3243 }
3244
3245 if (b_bool) {
3246 } else {
3247 guard1 = true;
3248 }
3249 } else {
3250 guard1 = true;
3251 }
3252
3253 if (guard1) {
3254 idx = 0;
3255 b_kstr = 1;
3256 exitg2 = false;
3257 while ((!exitg2) && (b_kstr - 1 < 6)) {
3258 if (!ubOK[b_kstr - 1]) {
3259 idx++;
3260 ii_data[idx - 1] = b_kstr;
3261 if (idx >= 6) {
3262 exitg2 = true;
3263 } else {
3264 b_kstr++;
3265 }
3266 } else {
3267 b_kstr++;
3268 }
3269 }
3270
3271 if (1 > idx) {
3272 idx = 0;
3273 }
3274
3275 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
3276 Q[ii_data[b_kstr] - 1] = limits->data[(ii_data[b_kstr] + limits->size[0])
3277 - 1];
3278 }
3279
3280 idx = 0;
3281 b_kstr = 1;
3282 exitg2 = false;
3283 while ((!exitg2) && (b_kstr - 1 < 6)) {
3284 if (!lbOK[b_kstr - 1]) {
3285 idx++;
3286 ii_data[idx - 1] = b_kstr;
3287 if (idx >= 6) {
3288 exitg2 = true;
3289 } else {
3290 b_kstr++;
3291 }
3292 } else {
3293 b_kstr++;
3294 }
3295 }
3296
3297 if (1 > idx) {
3298 idx = 0;
3299 }
3300
3301 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
3302 Q[ii_data[b_kstr] - 1] = limits->data[ii_data[b_kstr] - 1];
3303 }
3304 }
3305
3306 cartesian_trajec_emxFree_real_T(&limits);
3307}
3308
3309static boolean_T cartesian_trajectory_pla_strcmp(const
3310 emxArray_char_T_cartesian_tra_T *a, const emxArray_char_T_cartesian_tra_T *b)
3311{
3312 boolean_T b_bool;
3313 int32_T kstr;
3314 int32_T b_kstr;
3315 boolean_T d;
3316 int32_T exitg1;
3317 b_bool = false;
3318 d = (a->size[1] == 0);
3319 if (d && (b->size[1] == 0)) {
3320 b_bool = true;
3321 } else if (a->size[1] != b->size[1]) {
3322 } else {
3323 b_kstr = 1;
3324 do {
3325 exitg1 = 0;
3326 if (b_kstr - 1 <= b->size[1] - 1) {
3327 kstr = b_kstr - 1;
3328 if (a->data[kstr] != b->data[kstr]) {
3329 exitg1 = 1;
3330 } else {
3331 b_kstr++;
3332 }
3333 } else {
3334 b_bool = true;
3335 exitg1 = 1;
3336 }
3337 } while (exitg1 == 0);
3338 }
3339
3340 return b_bool;
3341}
3342
3343static void rigidBodyJoint_get_JointAxis_as(const
3344 c_rigidBodyJoint_cartesian_as_T *obj, real_T ax[3])
3345{
3346 emxArray_char_T_cartesian_tra_T *a;
3347 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
3348
3349 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
3350
3351 boolean_T guard1 = false;
3352 int32_T exitg1;
3353 cartesian_traj_emxInit_char_T_a(&a, 2);
3354 cartesian_trajectory_planner_B.b_kstr_e = a->size[0] * a->size[1];
3355 a->size[0] = 1;
3356 a->size[1] = obj->Type->size[1];
3357 cart_emxEnsureCapacity_char_T_a(a, cartesian_trajectory_planner_B.b_kstr_e);
3358 cartesian_trajectory_planner_B.loop_ub_n = obj->Type->size[0] * obj->
3359 Type->size[1] - 1;
3360 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
3361 cartesian_trajectory_planner_B.b_kstr_e <=
3362 cartesian_trajectory_planner_B.loop_ub_n;
3363 cartesian_trajectory_planner_B.b_kstr_e++) {
3364 a->data[cartesian_trajectory_planner_B.b_kstr_e] = obj->Type->
3365 data[cartesian_trajectory_planner_B.b_kstr_e];
3366 }
3367
3368 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
3369 cartesian_trajectory_planner_B.b_kstr_e < 8;
3370 cartesian_trajectory_planner_B.b_kstr_e++) {
3371 cartesian_trajectory_planner_B.b_fx[cartesian_trajectory_planner_B.b_kstr_e]
3372 = tmp[cartesian_trajectory_planner_B.b_kstr_e];
3373 }
3374
3375 cartesian_trajectory_planner_B.b_bool_d = false;
3376 if (a->size[1] == 8) {
3377 cartesian_trajectory_planner_B.b_kstr_e = 1;
3378 do {
3379 exitg1 = 0;
3380 if (cartesian_trajectory_planner_B.b_kstr_e - 1 < 8) {
3381 cartesian_trajectory_planner_B.loop_ub_n =
3382 cartesian_trajectory_planner_B.b_kstr_e - 1;
3383 if (a->data[cartesian_trajectory_planner_B.loop_ub_n] !=
3384 cartesian_trajectory_planner_B.b_fx[cartesian_trajectory_planner_B.loop_ub_n])
3385 {
3386 exitg1 = 1;
3387 } else {
3388 cartesian_trajectory_planner_B.b_kstr_e++;
3389 }
3390 } else {
3391 cartesian_trajectory_planner_B.b_bool_d = true;
3392 exitg1 = 1;
3393 }
3394 } while (exitg1 == 0);
3395 }
3396
3397 guard1 = false;
3398 if (cartesian_trajectory_planner_B.b_bool_d) {
3399 guard1 = true;
3400 } else {
3401 cartesian_trajectory_planner_B.b_kstr_e = a->size[0] * a->size[1];
3402 a->size[0] = 1;
3403 a->size[1] = obj->Type->size[1];
3404 cart_emxEnsureCapacity_char_T_a(a, cartesian_trajectory_planner_B.b_kstr_e);
3405 cartesian_trajectory_planner_B.loop_ub_n = obj->Type->size[0] * obj->
3406 Type->size[1] - 1;
3407 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
3408 cartesian_trajectory_planner_B.b_kstr_e <=
3409 cartesian_trajectory_planner_B.loop_ub_n;
3410 cartesian_trajectory_planner_B.b_kstr_e++) {
3411 a->data[cartesian_trajectory_planner_B.b_kstr_e] = obj->Type->
3412 data[cartesian_trajectory_planner_B.b_kstr_e];
3413 }
3414
3415 for (cartesian_trajectory_planner_B.b_kstr_e = 0;
3416 cartesian_trajectory_planner_B.b_kstr_e < 9;
3417 cartesian_trajectory_planner_B.b_kstr_e++) {
3418 cartesian_trajectory_planner_B.b_f[cartesian_trajectory_planner_B.b_kstr_e]
3419 = tmp_0[cartesian_trajectory_planner_B.b_kstr_e];
3420 }
3421
3422 cartesian_trajectory_planner_B.b_bool_d = false;
3423 if (a->size[1] == 9) {
3424 cartesian_trajectory_planner_B.b_kstr_e = 1;
3425 do {
3426 exitg1 = 0;
3427 if (cartesian_trajectory_planner_B.b_kstr_e - 1 < 9) {
3428 cartesian_trajectory_planner_B.loop_ub_n =
3429 cartesian_trajectory_planner_B.b_kstr_e - 1;
3430 if (a->data[cartesian_trajectory_planner_B.loop_ub_n] !=
3431 cartesian_trajectory_planner_B.b_f[cartesian_trajectory_planner_B.loop_ub_n])
3432 {
3433 exitg1 = 1;
3434 } else {
3435 cartesian_trajectory_planner_B.b_kstr_e++;
3436 }
3437 } else {
3438 cartesian_trajectory_planner_B.b_bool_d = true;
3439 exitg1 = 1;
3440 }
3441 } while (exitg1 == 0);
3442 }
3443
3444 if (cartesian_trajectory_planner_B.b_bool_d) {
3445 guard1 = true;
3446 } else {
3447 ax[0] = (rtNaN);
3448 ax[1] = (rtNaN);
3449 ax[2] = (rtNaN);
3450 }
3451 }
3452
3453 if (guard1) {
3454 ax[0] = obj->JointAxisInternal[0];
3455 ax[1] = obj->JointAxisInternal[1];
3456 ax[2] = obj->JointAxisInternal[2];
3457 }
3458
3459 cartesian_traj_emxFree_char_T_a(&a);
3460}
3461
3462static void cartesian_trajectory_planne_cat(real_T varargin_1, real_T varargin_2,
3463 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
3464 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9])
3465{
3466 y[0] = varargin_1;
3467 y[1] = varargin_2;
3468 y[2] = varargin_3;
3469 y[3] = varargin_4;
3470 y[4] = varargin_5;
3471 y[5] = varargin_6;
3472 y[6] = varargin_7;
3473 y[7] = varargin_8;
3474 y[8] = varargin_9;
3475}
3476
3477static void rigidBodyJoint_transformBodyT_a(const
3478 c_rigidBodyJoint_cartesian_as_T *obj, const real_T q_data[], const int32_T
3479 *q_size, real_T T[16])
3480{
3481 emxArray_char_T_cartesian_tra_T *switch_expression;
3482 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
3483
3484 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
3485
3486 int32_T exitg1;
3487 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
3488 cartesian_trajectory_planner_B.b_kstr_p = switch_expression->size[0] *
3489 switch_expression->size[1];
3490 switch_expression->size[0] = 1;
3491 switch_expression->size[1] = obj->Type->size[1];
3492 cart_emxEnsureCapacity_char_T_a(switch_expression,
3493 cartesian_trajectory_planner_B.b_kstr_p);
3494 cartesian_trajectory_planner_B.loop_ub_f = obj->Type->size[0] * obj->
3495 Type->size[1] - 1;
3496 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3497 cartesian_trajectory_planner_B.b_kstr_p <=
3498 cartesian_trajectory_planner_B.loop_ub_f;
3499 cartesian_trajectory_planner_B.b_kstr_p++) {
3500 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_p] = obj->
3501 Type->data[cartesian_trajectory_planner_B.b_kstr_p];
3502 }
3503
3504 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3505 cartesian_trajectory_planner_B.b_kstr_p < 5;
3506 cartesian_trajectory_planner_B.b_kstr_p++) {
3507 cartesian_trajectory_planner_B.b_j[cartesian_trajectory_planner_B.b_kstr_p] =
3508 tmp[cartesian_trajectory_planner_B.b_kstr_p];
3509 }
3510
3511 cartesian_trajectory_planner_B.b_bool_c = false;
3512 if (switch_expression->size[1] == 5) {
3513 cartesian_trajectory_planner_B.b_kstr_p = 1;
3514 do {
3515 exitg1 = 0;
3516 if (cartesian_trajectory_planner_B.b_kstr_p - 1 < 5) {
3517 cartesian_trajectory_planner_B.loop_ub_f =
3518 cartesian_trajectory_planner_B.b_kstr_p - 1;
3519 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_f] !=
3520 cartesian_trajectory_planner_B.b_j[cartesian_trajectory_planner_B.loop_ub_f])
3521 {
3522 exitg1 = 1;
3523 } else {
3524 cartesian_trajectory_planner_B.b_kstr_p++;
3525 }
3526 } else {
3527 cartesian_trajectory_planner_B.b_bool_c = true;
3528 exitg1 = 1;
3529 }
3530 } while (exitg1 == 0);
3531 }
3532
3533 if (cartesian_trajectory_planner_B.b_bool_c) {
3534 cartesian_trajectory_planner_B.b_kstr_p = 0;
3535 } else {
3536 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3537 cartesian_trajectory_planner_B.b_kstr_p < 8;
3538 cartesian_trajectory_planner_B.b_kstr_p++) {
3539 cartesian_trajectory_planner_B.b_c[cartesian_trajectory_planner_B.b_kstr_p]
3540 = tmp_0[cartesian_trajectory_planner_B.b_kstr_p];
3541 }
3542
3543 cartesian_trajectory_planner_B.b_bool_c = false;
3544 if (switch_expression->size[1] == 8) {
3545 cartesian_trajectory_planner_B.b_kstr_p = 1;
3546 do {
3547 exitg1 = 0;
3548 if (cartesian_trajectory_planner_B.b_kstr_p - 1 < 8) {
3549 cartesian_trajectory_planner_B.loop_ub_f =
3550 cartesian_trajectory_planner_B.b_kstr_p - 1;
3551 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_f]
3552 !=
3553 cartesian_trajectory_planner_B.b_c[cartesian_trajectory_planner_B.loop_ub_f])
3554 {
3555 exitg1 = 1;
3556 } else {
3557 cartesian_trajectory_planner_B.b_kstr_p++;
3558 }
3559 } else {
3560 cartesian_trajectory_planner_B.b_bool_c = true;
3561 exitg1 = 1;
3562 }
3563 } while (exitg1 == 0);
3564 }
3565
3566 if (cartesian_trajectory_planner_B.b_bool_c) {
3567 cartesian_trajectory_planner_B.b_kstr_p = 1;
3568 } else {
3569 cartesian_trajectory_planner_B.b_kstr_p = -1;
3570 }
3571 }
3572
3573 cartesian_traj_emxFree_char_T_a(&switch_expression);
3574 switch (cartesian_trajectory_planner_B.b_kstr_p) {
3575 case 0:
3576 memset(&cartesian_trajectory_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
3577 cartesian_trajectory_planner_B.TJ[0] = 1.0;
3578 cartesian_trajectory_planner_B.TJ[5] = 1.0;
3579 cartesian_trajectory_planner_B.TJ[10] = 1.0;
3580 cartesian_trajectory_planner_B.TJ[15] = 1.0;
3581 break;
3582
3583 case 1:
3584 rigidBodyJoint_get_JointAxis_as(obj, cartesian_trajectory_planner_B.v_ch);
3585 cartesian_trajectory_planner_B.loop_ub_f = (*q_size != 0) - 1;
3586 cartesian_trajectory_planner_B.result_data[0] =
3587 cartesian_trajectory_planner_B.v_ch[0];
3588 cartesian_trajectory_planner_B.result_data[1] =
3589 cartesian_trajectory_planner_B.v_ch[1];
3590 cartesian_trajectory_planner_B.result_data[2] =
3591 cartesian_trajectory_planner_B.v_ch[2];
3592 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3593 cartesian_trajectory_planner_B.b_kstr_p <=
3594 cartesian_trajectory_planner_B.loop_ub_f;
3595 cartesian_trajectory_planner_B.b_kstr_p++) {
3596 cartesian_trajectory_planner_B.result_data[3] = q_data[0];
3597 }
3598
3599 cartesian_trajectory_planner_B.cth = 1.0 / sqrt
3600 ((cartesian_trajectory_planner_B.result_data[0] *
3601 cartesian_trajectory_planner_B.result_data[0] +
3602 cartesian_trajectory_planner_B.result_data[1] *
3603 cartesian_trajectory_planner_B.result_data[1]) +
3604 cartesian_trajectory_planner_B.result_data[2] *
3605 cartesian_trajectory_planner_B.result_data[2]);
3606 cartesian_trajectory_planner_B.v_ch[0] =
3607 cartesian_trajectory_planner_B.result_data[0] *
3608 cartesian_trajectory_planner_B.cth;
3609 cartesian_trajectory_planner_B.v_ch[1] =
3610 cartesian_trajectory_planner_B.result_data[1] *
3611 cartesian_trajectory_planner_B.cth;
3612 cartesian_trajectory_planner_B.v_ch[2] =
3613 cartesian_trajectory_planner_B.result_data[2] *
3614 cartesian_trajectory_planner_B.cth;
3615 cartesian_trajectory_planner_B.cth = cos
3616 (cartesian_trajectory_planner_B.result_data[3]);
3617 cartesian_trajectory_planner_B.sth = sin
3618 (cartesian_trajectory_planner_B.result_data[3]);
3619 cartesian_trajectory_planner_B.tempR_tmp =
3620 cartesian_trajectory_planner_B.v_ch[1] *
3621 cartesian_trajectory_planner_B.v_ch[0] * (1.0 -
3622 cartesian_trajectory_planner_B.cth);
3623 cartesian_trajectory_planner_B.tempR_tmp_f =
3624 cartesian_trajectory_planner_B.v_ch[2] *
3625 cartesian_trajectory_planner_B.sth;
3626 cartesian_trajectory_planner_B.tempR_tmp_d =
3627 cartesian_trajectory_planner_B.v_ch[2] *
3628 cartesian_trajectory_planner_B.v_ch[0] * (1.0 -
3629 cartesian_trajectory_planner_B.cth);
3630 cartesian_trajectory_planner_B.tempR_tmp_j =
3631 cartesian_trajectory_planner_B.v_ch[1] *
3632 cartesian_trajectory_planner_B.sth;
3633 cartesian_trajectory_planner_B.tempR_tmp_i =
3634 cartesian_trajectory_planner_B.v_ch[2] *
3635 cartesian_trajectory_planner_B.v_ch[1] * (1.0 -
3636 cartesian_trajectory_planner_B.cth);
3637 cartesian_trajectory_planner_B.sth *= cartesian_trajectory_planner_B.v_ch[0];
3638 cartesian_trajectory_planne_cat(cartesian_trajectory_planner_B.v_ch[0] *
3639 cartesian_trajectory_planner_B.v_ch[0] * (1.0 -
3640 cartesian_trajectory_planner_B.cth) + cartesian_trajectory_planner_B.cth,
3641 cartesian_trajectory_planner_B.tempR_tmp -
3642 cartesian_trajectory_planner_B.tempR_tmp_f,
3643 cartesian_trajectory_planner_B.tempR_tmp_d +
3644 cartesian_trajectory_planner_B.tempR_tmp_j,
3645 cartesian_trajectory_planner_B.tempR_tmp +
3646 cartesian_trajectory_planner_B.tempR_tmp_f,
3647 cartesian_trajectory_planner_B.v_ch[1] *
3648 cartesian_trajectory_planner_B.v_ch[1] * (1.0 -
3649 cartesian_trajectory_planner_B.cth) + cartesian_trajectory_planner_B.cth,
3650 cartesian_trajectory_planner_B.tempR_tmp_i -
3651 cartesian_trajectory_planner_B.sth,
3652 cartesian_trajectory_planner_B.tempR_tmp_d -
3653 cartesian_trajectory_planner_B.tempR_tmp_j,
3654 cartesian_trajectory_planner_B.tempR_tmp_i +
3655 cartesian_trajectory_planner_B.sth, cartesian_trajectory_planner_B.v_ch[2]
3656 * cartesian_trajectory_planner_B.v_ch[2] * (1.0 -
3657 cartesian_trajectory_planner_B.cth) + cartesian_trajectory_planner_B.cth,
3658 cartesian_trajectory_planner_B.tempR_i);
3659 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3660 cartesian_trajectory_planner_B.b_kstr_p < 3;
3661 cartesian_trajectory_planner_B.b_kstr_p++) {
3662 cartesian_trajectory_planner_B.loop_ub_f =
3663 cartesian_trajectory_planner_B.b_kstr_p + 1;
3664 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_f
3665 - 1] = cartesian_trajectory_planner_B.tempR_i
3666 [(cartesian_trajectory_planner_B.loop_ub_f - 1) * 3];
3667 cartesian_trajectory_planner_B.loop_ub_f =
3668 cartesian_trajectory_planner_B.b_kstr_p + 1;
3669 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_f
3670 + 2] = cartesian_trajectory_planner_B.tempR_i
3671 [(cartesian_trajectory_planner_B.loop_ub_f - 1) * 3 + 1];
3672 cartesian_trajectory_planner_B.loop_ub_f =
3673 cartesian_trajectory_planner_B.b_kstr_p + 1;
3674 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_f
3675 + 5] = cartesian_trajectory_planner_B.tempR_i
3676 [(cartesian_trajectory_planner_B.loop_ub_f - 1) * 3 + 2];
3677 }
3678
3679 memset(&cartesian_trajectory_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
3680 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3681 cartesian_trajectory_planner_B.b_kstr_p < 3;
3682 cartesian_trajectory_planner_B.b_kstr_p++) {
3683 cartesian_trajectory_planner_B.loop_ub_f =
3684 cartesian_trajectory_planner_B.b_kstr_p << 2;
3685 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_f]
3686 = cartesian_trajectory_planner_B.R_f[3 *
3687 cartesian_trajectory_planner_B.b_kstr_p];
3688 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_f
3689 + 1] = cartesian_trajectory_planner_B.R_f[3 *
3690 cartesian_trajectory_planner_B.b_kstr_p + 1];
3691 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_f
3692 + 2] = cartesian_trajectory_planner_B.R_f[3 *
3693 cartesian_trajectory_planner_B.b_kstr_p + 2];
3694 }
3695
3696 cartesian_trajectory_planner_B.TJ[15] = 1.0;
3697 break;
3698
3699 default:
3700 rigidBodyJoint_get_JointAxis_as(obj, cartesian_trajectory_planner_B.v_ch);
3701 memset(&cartesian_trajectory_planner_B.tempR_i[0], 0, 9U * sizeof(real_T));
3702 cartesian_trajectory_planner_B.tempR_i[0] = 1.0;
3703 cartesian_trajectory_planner_B.tempR_i[4] = 1.0;
3704 cartesian_trajectory_planner_B.tempR_i[8] = 1.0;
3705 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3706 cartesian_trajectory_planner_B.b_kstr_p < 3;
3707 cartesian_trajectory_planner_B.b_kstr_p++) {
3708 cartesian_trajectory_planner_B.loop_ub_f =
3709 cartesian_trajectory_planner_B.b_kstr_p << 2;
3710 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_f]
3711 = cartesian_trajectory_planner_B.tempR_i[3 *
3712 cartesian_trajectory_planner_B.b_kstr_p];
3713 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_f
3714 + 1] = cartesian_trajectory_planner_B.tempR_i[3 *
3715 cartesian_trajectory_planner_B.b_kstr_p + 1];
3716 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_f
3717 + 2] = cartesian_trajectory_planner_B.tempR_i[3 *
3718 cartesian_trajectory_planner_B.b_kstr_p + 2];
3719 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.b_kstr_p
3720 + 12] =
3721 cartesian_trajectory_planner_B.v_ch[cartesian_trajectory_planner_B.b_kstr_p]
3722 * q_data[0];
3723 }
3724
3725 cartesian_trajectory_planner_B.TJ[3] = 0.0;
3726 cartesian_trajectory_planner_B.TJ[7] = 0.0;
3727 cartesian_trajectory_planner_B.TJ[11] = 0.0;
3728 cartesian_trajectory_planner_B.TJ[15] = 1.0;
3729 break;
3730 }
3731
3732 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3733 cartesian_trajectory_planner_B.b_kstr_p < 16;
3734 cartesian_trajectory_planner_B.b_kstr_p++) {
3735 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_p] =
3736 obj->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_p];
3737 }
3738
3739 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3740 cartesian_trajectory_planner_B.b_kstr_p < 16;
3741 cartesian_trajectory_planner_B.b_kstr_p++) {
3742 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.b_kstr_p] =
3743 obj->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_p];
3744 }
3745
3746 for (cartesian_trajectory_planner_B.b_kstr_p = 0;
3747 cartesian_trajectory_planner_B.b_kstr_p < 4;
3748 cartesian_trajectory_planner_B.b_kstr_p++) {
3749 for (cartesian_trajectory_planner_B.loop_ub_f = 0;
3750 cartesian_trajectory_planner_B.loop_ub_f < 4;
3751 cartesian_trajectory_planner_B.loop_ub_f++) {
3752 cartesian_trajectory_planner_B.a_tmp_tmp =
3753 cartesian_trajectory_planner_B.loop_ub_f << 2;
3754 cartesian_trajectory_planner_B.a_tmp =
3755 cartesian_trajectory_planner_B.b_kstr_p +
3756 cartesian_trajectory_planner_B.a_tmp_tmp;
3757 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] =
3758 0.0;
3759 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] +=
3760 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp]
3761 * cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_p];
3762 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] +=
3763 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp
3764 + 1] *
3765 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_p
3766 + 4];
3767 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] +=
3768 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp
3769 + 2] *
3770 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_p
3771 + 8];
3772 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] +=
3773 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp
3774 + 3] *
3775 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_p
3776 + 12];
3777 }
3778
3779 for (cartesian_trajectory_planner_B.loop_ub_f = 0;
3780 cartesian_trajectory_planner_B.loop_ub_f < 4;
3781 cartesian_trajectory_planner_B.loop_ub_f++) {
3782 cartesian_trajectory_planner_B.a_tmp_tmp =
3783 cartesian_trajectory_planner_B.loop_ub_f << 2;
3784 cartesian_trajectory_planner_B.a_tmp =
3785 cartesian_trajectory_planner_B.b_kstr_p +
3786 cartesian_trajectory_planner_B.a_tmp_tmp;
3787 T[cartesian_trajectory_planner_B.a_tmp] = 0.0;
3788 T[cartesian_trajectory_planner_B.a_tmp] +=
3789 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp]
3790 * cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.b_kstr_p];
3791 T[cartesian_trajectory_planner_B.a_tmp] +=
3792 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp
3793 + 1] *
3794 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.b_kstr_p
3795 + 4];
3796 T[cartesian_trajectory_planner_B.a_tmp] +=
3797 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp
3798 + 2] *
3799 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.b_kstr_p
3800 + 8];
3801 T[cartesian_trajectory_planner_B.a_tmp] +=
3802 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp
3803 + 3] *
3804 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.b_kstr_p
3805 + 12];
3806 }
3807 }
3808}
3809
3810static void rigidBodyJoint_transformBodyToP(const
3811 c_rigidBodyJoint_cartesian_as_T *obj, real_T T[16])
3812{
3813 emxArray_char_T_cartesian_tra_T *switch_expression;
3814 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
3815
3816 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
3817
3818 int32_T exitg1;
3819 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
3820 cartesian_trajectory_planner_B.b_kstr_g = switch_expression->size[0] *
3821 switch_expression->size[1];
3822 switch_expression->size[0] = 1;
3823 switch_expression->size[1] = obj->Type->size[1];
3824 cart_emxEnsureCapacity_char_T_a(switch_expression,
3825 cartesian_trajectory_planner_B.b_kstr_g);
3826 cartesian_trajectory_planner_B.loop_ub_l = obj->Type->size[0] * obj->
3827 Type->size[1] - 1;
3828 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
3829 cartesian_trajectory_planner_B.b_kstr_g <=
3830 cartesian_trajectory_planner_B.loop_ub_l;
3831 cartesian_trajectory_planner_B.b_kstr_g++) {
3832 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_g] = obj->
3833 Type->data[cartesian_trajectory_planner_B.b_kstr_g];
3834 }
3835
3836 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
3837 cartesian_trajectory_planner_B.b_kstr_g < 5;
3838 cartesian_trajectory_planner_B.b_kstr_g++) {
3839 cartesian_trajectory_planner_B.b_fr[cartesian_trajectory_planner_B.b_kstr_g]
3840 = tmp[cartesian_trajectory_planner_B.b_kstr_g];
3841 }
3842
3843 cartesian_trajectory_planner_B.b_bool_i = false;
3844 if (switch_expression->size[1] == 5) {
3845 cartesian_trajectory_planner_B.b_kstr_g = 1;
3846 do {
3847 exitg1 = 0;
3848 if (cartesian_trajectory_planner_B.b_kstr_g - 1 < 5) {
3849 cartesian_trajectory_planner_B.loop_ub_l =
3850 cartesian_trajectory_planner_B.b_kstr_g - 1;
3851 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_l] !=
3852 cartesian_trajectory_planner_B.b_fr[cartesian_trajectory_planner_B.loop_ub_l])
3853 {
3854 exitg1 = 1;
3855 } else {
3856 cartesian_trajectory_planner_B.b_kstr_g++;
3857 }
3858 } else {
3859 cartesian_trajectory_planner_B.b_bool_i = true;
3860 exitg1 = 1;
3861 }
3862 } while (exitg1 == 0);
3863 }
3864
3865 if (cartesian_trajectory_planner_B.b_bool_i) {
3866 cartesian_trajectory_planner_B.b_kstr_g = 0;
3867 } else {
3868 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
3869 cartesian_trajectory_planner_B.b_kstr_g < 8;
3870 cartesian_trajectory_planner_B.b_kstr_g++) {
3871 cartesian_trajectory_planner_B.b_nj[cartesian_trajectory_planner_B.b_kstr_g]
3872 = tmp_0[cartesian_trajectory_planner_B.b_kstr_g];
3873 }
3874
3875 cartesian_trajectory_planner_B.b_bool_i = false;
3876 if (switch_expression->size[1] == 8) {
3877 cartesian_trajectory_planner_B.b_kstr_g = 1;
3878 do {
3879 exitg1 = 0;
3880 if (cartesian_trajectory_planner_B.b_kstr_g - 1 < 8) {
3881 cartesian_trajectory_planner_B.loop_ub_l =
3882 cartesian_trajectory_planner_B.b_kstr_g - 1;
3883 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_l]
3884 !=
3885 cartesian_trajectory_planner_B.b_nj[cartesian_trajectory_planner_B.loop_ub_l])
3886 {
3887 exitg1 = 1;
3888 } else {
3889 cartesian_trajectory_planner_B.b_kstr_g++;
3890 }
3891 } else {
3892 cartesian_trajectory_planner_B.b_bool_i = true;
3893 exitg1 = 1;
3894 }
3895 } while (exitg1 == 0);
3896 }
3897
3898 if (cartesian_trajectory_planner_B.b_bool_i) {
3899 cartesian_trajectory_planner_B.b_kstr_g = 1;
3900 } else {
3901 cartesian_trajectory_planner_B.b_kstr_g = -1;
3902 }
3903 }
3904
3905 cartesian_traj_emxFree_char_T_a(&switch_expression);
3906 switch (cartesian_trajectory_planner_B.b_kstr_g) {
3907 case 0:
3908 memset(&cartesian_trajectory_planner_B.TJ_d[0], 0, sizeof(real_T) << 4U);
3909 cartesian_trajectory_planner_B.TJ_d[0] = 1.0;
3910 cartesian_trajectory_planner_B.TJ_d[5] = 1.0;
3911 cartesian_trajectory_planner_B.TJ_d[10] = 1.0;
3912 cartesian_trajectory_planner_B.TJ_d[15] = 1.0;
3913 break;
3914
3915 case 1:
3916 rigidBodyJoint_get_JointAxis_as(obj, cartesian_trajectory_planner_B.v_n);
3917 cartesian_trajectory_planner_B.axang_idx_0 =
3918 cartesian_trajectory_planner_B.v_n[0];
3919 cartesian_trajectory_planner_B.axang_idx_1 =
3920 cartesian_trajectory_planner_B.v_n[1];
3921 cartesian_trajectory_planner_B.axang_idx_2 =
3922 cartesian_trajectory_planner_B.v_n[2];
3923 cartesian_trajectory_planner_B.b_l0 = 1.0 / sqrt
3924 ((cartesian_trajectory_planner_B.axang_idx_0 *
3925 cartesian_trajectory_planner_B.axang_idx_0 +
3926 cartesian_trajectory_planner_B.axang_idx_1 *
3927 cartesian_trajectory_planner_B.axang_idx_1) +
3928 cartesian_trajectory_planner_B.axang_idx_2 *
3929 cartesian_trajectory_planner_B.axang_idx_2);
3930 cartesian_trajectory_planner_B.v_n[0] =
3931 cartesian_trajectory_planner_B.axang_idx_0 *
3932 cartesian_trajectory_planner_B.b_l0;
3933 cartesian_trajectory_planner_B.v_n[1] =
3934 cartesian_trajectory_planner_B.axang_idx_1 *
3935 cartesian_trajectory_planner_B.b_l0;
3936 cartesian_trajectory_planner_B.v_n[2] =
3937 cartesian_trajectory_planner_B.axang_idx_2 *
3938 cartesian_trajectory_planner_B.b_l0;
3939 cartesian_trajectory_planner_B.axang_idx_0 =
3940 cartesian_trajectory_planner_B.v_n[1] *
3941 cartesian_trajectory_planner_B.v_n[0] * 0.0;
3942 cartesian_trajectory_planner_B.axang_idx_1 =
3943 cartesian_trajectory_planner_B.v_n[2] *
3944 cartesian_trajectory_planner_B.v_n[0] * 0.0;
3945 cartesian_trajectory_planner_B.axang_idx_2 =
3946 cartesian_trajectory_planner_B.v_n[2] *
3947 cartesian_trajectory_planner_B.v_n[1] * 0.0;
3948 cartesian_trajectory_planne_cat(cartesian_trajectory_planner_B.v_n[0] *
3949 cartesian_trajectory_planner_B.v_n[0] * 0.0 + 1.0,
3950 cartesian_trajectory_planner_B.axang_idx_0 -
3951 cartesian_trajectory_planner_B.v_n[2] * 0.0,
3952 cartesian_trajectory_planner_B.axang_idx_1 +
3953 cartesian_trajectory_planner_B.v_n[1] * 0.0,
3954 cartesian_trajectory_planner_B.axang_idx_0 +
3955 cartesian_trajectory_planner_B.v_n[2] * 0.0,
3956 cartesian_trajectory_planner_B.v_n[1] *
3957 cartesian_trajectory_planner_B.v_n[1] * 0.0 + 1.0,
3958 cartesian_trajectory_planner_B.axang_idx_2 -
3959 cartesian_trajectory_planner_B.v_n[0] * 0.0,
3960 cartesian_trajectory_planner_B.axang_idx_1 -
3961 cartesian_trajectory_planner_B.v_n[1] * 0.0,
3962 cartesian_trajectory_planner_B.axang_idx_2 +
3963 cartesian_trajectory_planner_B.v_n[0] * 0.0,
3964 cartesian_trajectory_planner_B.v_n[2] *
3965 cartesian_trajectory_planner_B.v_n[2] * 0.0 + 1.0,
3966 cartesian_trajectory_planner_B.tempR_g);
3967 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
3968 cartesian_trajectory_planner_B.b_kstr_g < 3;
3969 cartesian_trajectory_planner_B.b_kstr_g++) {
3970 cartesian_trajectory_planner_B.loop_ub_l =
3971 cartesian_trajectory_planner_B.b_kstr_g + 1;
3972 cartesian_trajectory_planner_B.R_ff[cartesian_trajectory_planner_B.loop_ub_l
3973 - 1] = cartesian_trajectory_planner_B.tempR_g
3974 [(cartesian_trajectory_planner_B.loop_ub_l - 1) * 3];
3975 cartesian_trajectory_planner_B.loop_ub_l =
3976 cartesian_trajectory_planner_B.b_kstr_g + 1;
3977 cartesian_trajectory_planner_B.R_ff[cartesian_trajectory_planner_B.loop_ub_l
3978 + 2] = cartesian_trajectory_planner_B.tempR_g
3979 [(cartesian_trajectory_planner_B.loop_ub_l - 1) * 3 + 1];
3980 cartesian_trajectory_planner_B.loop_ub_l =
3981 cartesian_trajectory_planner_B.b_kstr_g + 1;
3982 cartesian_trajectory_planner_B.R_ff[cartesian_trajectory_planner_B.loop_ub_l
3983 + 5] = cartesian_trajectory_planner_B.tempR_g
3984 [(cartesian_trajectory_planner_B.loop_ub_l - 1) * 3 + 2];
3985 }
3986
3987 memset(&cartesian_trajectory_planner_B.TJ_d[0], 0, sizeof(real_T) << 4U);
3988 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
3989 cartesian_trajectory_planner_B.b_kstr_g < 3;
3990 cartesian_trajectory_planner_B.b_kstr_g++) {
3991 cartesian_trajectory_planner_B.loop_ub_l =
3992 cartesian_trajectory_planner_B.b_kstr_g << 2;
3993 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_l]
3994 = cartesian_trajectory_planner_B.R_ff[3 *
3995 cartesian_trajectory_planner_B.b_kstr_g];
3996 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_l
3997 + 1] = cartesian_trajectory_planner_B.R_ff[3 *
3998 cartesian_trajectory_planner_B.b_kstr_g + 1];
3999 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_l
4000 + 2] = cartesian_trajectory_planner_B.R_ff[3 *
4001 cartesian_trajectory_planner_B.b_kstr_g + 2];
4002 }
4003
4004 cartesian_trajectory_planner_B.TJ_d[15] = 1.0;
4005 break;
4006
4007 default:
4008 rigidBodyJoint_get_JointAxis_as(obj, cartesian_trajectory_planner_B.v_n);
4009 memset(&cartesian_trajectory_planner_B.tempR_g[0], 0, 9U * sizeof(real_T));
4010 cartesian_trajectory_planner_B.tempR_g[0] = 1.0;
4011 cartesian_trajectory_planner_B.tempR_g[4] = 1.0;
4012 cartesian_trajectory_planner_B.tempR_g[8] = 1.0;
4013 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
4014 cartesian_trajectory_planner_B.b_kstr_g < 3;
4015 cartesian_trajectory_planner_B.b_kstr_g++) {
4016 cartesian_trajectory_planner_B.loop_ub_l =
4017 cartesian_trajectory_planner_B.b_kstr_g << 2;
4018 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_l]
4019 = cartesian_trajectory_planner_B.tempR_g[3 *
4020 cartesian_trajectory_planner_B.b_kstr_g];
4021 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_l
4022 + 1] = cartesian_trajectory_planner_B.tempR_g[3 *
4023 cartesian_trajectory_planner_B.b_kstr_g + 1];
4024 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_l
4025 + 2] = cartesian_trajectory_planner_B.tempR_g[3 *
4026 cartesian_trajectory_planner_B.b_kstr_g + 2];
4027 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.b_kstr_g
4028 + 12] =
4029 cartesian_trajectory_planner_B.v_n[cartesian_trajectory_planner_B.b_kstr_g]
4030 * 0.0;
4031 }
4032
4033 cartesian_trajectory_planner_B.TJ_d[3] = 0.0;
4034 cartesian_trajectory_planner_B.TJ_d[7] = 0.0;
4035 cartesian_trajectory_planner_B.TJ_d[11] = 0.0;
4036 cartesian_trajectory_planner_B.TJ_d[15] = 1.0;
4037 break;
4038 }
4039
4040 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
4041 cartesian_trajectory_planner_B.b_kstr_g < 16;
4042 cartesian_trajectory_planner_B.b_kstr_g++) {
4043 cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_g] =
4044 obj->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_g];
4045 }
4046
4047 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
4048 cartesian_trajectory_planner_B.b_kstr_g < 16;
4049 cartesian_trajectory_planner_B.b_kstr_g++) {
4050 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.b_kstr_g] =
4051 obj->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_g];
4052 }
4053
4054 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
4055 cartesian_trajectory_planner_B.b_kstr_g < 4;
4056 cartesian_trajectory_planner_B.b_kstr_g++) {
4057 for (cartesian_trajectory_planner_B.loop_ub_l = 0;
4058 cartesian_trajectory_planner_B.loop_ub_l < 4;
4059 cartesian_trajectory_planner_B.loop_ub_l++) {
4060 cartesian_trajectory_planner_B.a_tmp_tmp_n =
4061 cartesian_trajectory_planner_B.loop_ub_l << 2;
4062 cartesian_trajectory_planner_B.a_tmp_m =
4063 cartesian_trajectory_planner_B.b_kstr_g +
4064 cartesian_trajectory_planner_B.a_tmp_tmp_n;
4065 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_m]
4066 = 0.0;
4067 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_m]
4068 +=
4069 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.a_tmp_tmp_n]
4070 * cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_g];
4071 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_m]
4072 +=
4073 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.a_tmp_tmp_n
4074 + 1] *
4075 cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_g
4076 + 4];
4077 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_m]
4078 +=
4079 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.a_tmp_tmp_n
4080 + 2] *
4081 cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_g
4082 + 8];
4083 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_m]
4084 +=
4085 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.a_tmp_tmp_n
4086 + 3] *
4087 cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_g
4088 + 12];
4089 }
4090
4091 for (cartesian_trajectory_planner_B.loop_ub_l = 0;
4092 cartesian_trajectory_planner_B.loop_ub_l < 4;
4093 cartesian_trajectory_planner_B.loop_ub_l++) {
4094 cartesian_trajectory_planner_B.a_tmp_tmp_n =
4095 cartesian_trajectory_planner_B.loop_ub_l << 2;
4096 cartesian_trajectory_planner_B.a_tmp_m =
4097 cartesian_trajectory_planner_B.b_kstr_g +
4098 cartesian_trajectory_planner_B.a_tmp_tmp_n;
4099 T[cartesian_trajectory_planner_B.a_tmp_m] = 0.0;
4100 T[cartesian_trajectory_planner_B.a_tmp_m] +=
4101 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.a_tmp_tmp_n]
4102 * cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_g];
4103 T[cartesian_trajectory_planner_B.a_tmp_m] +=
4104 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.a_tmp_tmp_n
4105 + 1] *
4106 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_g
4107 + 4];
4108 T[cartesian_trajectory_planner_B.a_tmp_m] +=
4109 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.a_tmp_tmp_n
4110 + 2] *
4111 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_g
4112 + 8];
4113 T[cartesian_trajectory_planner_B.a_tmp_m] +=
4114 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.a_tmp_tmp_n
4115 + 3] *
4116 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_g
4117 + 12];
4118 }
4119 }
4120}
4121
4122static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
4123 const real_T qv[6], const emxArray_char_T_cartesian_tra_T *body1Name, real_T
4124 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_tra_T *Jac)
4125{
4126 w_robotics_manip_internal_Rig_T *body1;
4127 w_robotics_manip_internal_Rig_T *body2;
4128 emxArray_real_T_cartesian_tra_T *kinematicPathIndices;
4129 c_rigidBodyJoint_cartesian_as_T *joint;
4130 emxArray_char_T_cartesian_tra_T *body2Name;
4131 emxArray_real_T_cartesian_tra_T *ancestorIndices1;
4132 emxArray_real_T_cartesian_tra_T *ancestorIndices2;
4133 emxArray_real_T_cartesian_tra_T *y;
4134 emxArray_real_T_cartesian_tra_T *b;
4135 w_robotics_manip_internal_Rig_T *body;
4136 emxArray_char_T_cartesian_tra_T *bname;
4137 emxArray_real_T_cartesian_tra_T *ancestorIndices1_0;
4138 emxArray_real_T_cartesian_tra_T *ancestorIndices2_0;
4139 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
4140
4141 boolean_T exitg1;
4142 int32_T exitg2;
4143 cartesian_traj_emxInit_char_T_a(&body2Name, 2);
4144 cartesian_trajectory_planner_B.b_kstr = body2Name->size[0] * body2Name->size[1];
4145 body2Name->size[0] = 1;
4146 body2Name->size[1] = obj->Base.NameInternal->size[1];
4147 cart_emxEnsureCapacity_char_T_a(body2Name,
4148 cartesian_trajectory_planner_B.b_kstr);
4149 cartesian_trajectory_planner_B.loop_ub_i = obj->Base.NameInternal->size[0] *
4150 obj->Base.NameInternal->size[1] - 1;
4151 for (cartesian_trajectory_planner_B.b_kstr = 0;
4152 cartesian_trajectory_planner_B.b_kstr <=
4153 cartesian_trajectory_planner_B.loop_ub_i;
4154 cartesian_trajectory_planner_B.b_kstr++) {
4155 body2Name->data[cartesian_trajectory_planner_B.b_kstr] =
4156 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
4157 }
4158
4159 cartesian_traj_emxInit_char_T_a(&bname, 2);
4160 cartesian_trajectory_planner_B.bid1 = -1.0;
4161 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
4162 bname->size[0] = 1;
4163 bname->size[1] = obj->Base.NameInternal->size[1];
4164 cart_emxEnsureCapacity_char_T_a(bname, cartesian_trajectory_planner_B.b_kstr);
4165 cartesian_trajectory_planner_B.loop_ub_i = obj->Base.NameInternal->size[0] *
4166 obj->Base.NameInternal->size[1] - 1;
4167 for (cartesian_trajectory_planner_B.b_kstr = 0;
4168 cartesian_trajectory_planner_B.b_kstr <=
4169 cartesian_trajectory_planner_B.loop_ub_i;
4170 cartesian_trajectory_planner_B.b_kstr++) {
4171 bname->data[cartesian_trajectory_planner_B.b_kstr] = obj->
4172 Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
4173 }
4174
4175 if (cartesian_trajectory_pla_strcmp(bname, body1Name)) {
4176 cartesian_trajectory_planner_B.bid1 = 0.0;
4177 } else {
4178 cartesian_trajectory_planner_B.qidx_idx_1 = obj->NumBodies;
4179 cartesian_trajectory_planner_B.b_i_c = 0;
4180 exitg1 = false;
4181 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_c <=
4182 static_cast<int32_T>
4183 (cartesian_trajectory_planner_B.qidx_idx_1) - 1)) {
4184 body1 = obj->Bodies[cartesian_trajectory_planner_B.b_i_c];
4185 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
4186 bname->size[0] = 1;
4187 bname->size[1] = body1->NameInternal->size[1];
4188 cart_emxEnsureCapacity_char_T_a(bname,
4189 cartesian_trajectory_planner_B.b_kstr);
4190 cartesian_trajectory_planner_B.loop_ub_i = body1->NameInternal->size[0] *
4191 body1->NameInternal->size[1] - 1;
4192 for (cartesian_trajectory_planner_B.b_kstr = 0;
4193 cartesian_trajectory_planner_B.b_kstr <=
4194 cartesian_trajectory_planner_B.loop_ub_i;
4195 cartesian_trajectory_planner_B.b_kstr++) {
4196 bname->data[cartesian_trajectory_planner_B.b_kstr] = body1->
4197 NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
4198 }
4199
4200 if (cartesian_trajectory_pla_strcmp(bname, body1Name)) {
4201 cartesian_trajectory_planner_B.bid1 = static_cast<real_T>
4202 (cartesian_trajectory_planner_B.b_i_c) + 1.0;
4203 exitg1 = true;
4204 } else {
4205 cartesian_trajectory_planner_B.b_i_c++;
4206 }
4207 }
4208 }
4209
4210 cartesian_trajectory_planner_B.bid2 = -1.0;
4211 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
4212 bname->size[0] = 1;
4213 bname->size[1] = obj->Base.NameInternal->size[1];
4214 cart_emxEnsureCapacity_char_T_a(bname, cartesian_trajectory_planner_B.b_kstr);
4215 cartesian_trajectory_planner_B.loop_ub_i = obj->Base.NameInternal->size[0] *
4216 obj->Base.NameInternal->size[1] - 1;
4217 for (cartesian_trajectory_planner_B.b_kstr = 0;
4218 cartesian_trajectory_planner_B.b_kstr <=
4219 cartesian_trajectory_planner_B.loop_ub_i;
4220 cartesian_trajectory_planner_B.b_kstr++) {
4221 bname->data[cartesian_trajectory_planner_B.b_kstr] = obj->
4222 Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
4223 }
4224
4225 if (cartesian_trajectory_pla_strcmp(bname, body2Name)) {
4226 cartesian_trajectory_planner_B.bid2 = 0.0;
4227 } else {
4228 cartesian_trajectory_planner_B.qidx_idx_1 = obj->NumBodies;
4229 cartesian_trajectory_planner_B.b_i_c = 0;
4230 exitg1 = false;
4231 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_c <=
4232 static_cast<int32_T>
4233 (cartesian_trajectory_planner_B.qidx_idx_1) - 1)) {
4234 body1 = obj->Bodies[cartesian_trajectory_planner_B.b_i_c];
4235 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
4236 bname->size[0] = 1;
4237 bname->size[1] = body1->NameInternal->size[1];
4238 cart_emxEnsureCapacity_char_T_a(bname,
4239 cartesian_trajectory_planner_B.b_kstr);
4240 cartesian_trajectory_planner_B.loop_ub_i = body1->NameInternal->size[0] *
4241 body1->NameInternal->size[1] - 1;
4242 for (cartesian_trajectory_planner_B.b_kstr = 0;
4243 cartesian_trajectory_planner_B.b_kstr <=
4244 cartesian_trajectory_planner_B.loop_ub_i;
4245 cartesian_trajectory_planner_B.b_kstr++) {
4246 bname->data[cartesian_trajectory_planner_B.b_kstr] = body1->
4247 NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
4248 }
4249
4250 if (cartesian_trajectory_pla_strcmp(bname, body2Name)) {
4251 cartesian_trajectory_planner_B.bid2 = static_cast<real_T>
4252 (cartesian_trajectory_planner_B.b_i_c) + 1.0;
4253 exitg1 = true;
4254 } else {
4255 cartesian_trajectory_planner_B.b_i_c++;
4256 }
4257 }
4258 }
4259
4260 cartesian_traj_emxFree_char_T_a(&bname);
4261 if (cartesian_trajectory_planner_B.bid1 == 0.0) {
4262 body1 = &obj->Base;
4263 } else {
4264 body1 = obj->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.bid1)
4265 - 1];
4266 }
4267
4268 if (cartesian_trajectory_planner_B.bid2 == 0.0) {
4269 body2 = &obj->Base;
4270 } else {
4271 body2 = obj->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.bid2)
4272 - 1];
4273 }
4274
4275 cartesian_trajec_emxInit_real_T(&ancestorIndices1, 2);
4276 body = body1;
4277 cartesian_trajectory_planner_B.b_kstr = ancestorIndices1->size[0] *
4278 ancestorIndices1->size[1];
4279 ancestorIndices1->size[0] = 1;
4280 ancestorIndices1->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
4281 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
4282 cartesian_trajectory_planner_B.b_kstr);
4283 cartesian_trajectory_planner_B.loop_ub_i = static_cast<int32_T>(obj->NumBodies
4284 + 1.0) - 1;
4285 for (cartesian_trajectory_planner_B.b_kstr = 0;
4286 cartesian_trajectory_planner_B.b_kstr <=
4287 cartesian_trajectory_planner_B.loop_ub_i;
4288 cartesian_trajectory_planner_B.b_kstr++) {
4289 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr] = 0.0;
4290 }
4291
4292 cartesian_trajectory_planner_B.bid1 = 2.0;
4293 ancestorIndices1->data[0] = body1->Index;
4294 while (body->ParentIndex > 0.0) {
4295 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
4296 ancestorIndices1->data[static_cast<int32_T>
4297 (cartesian_trajectory_planner_B.bid1) - 1] = body->Index;
4298 cartesian_trajectory_planner_B.bid1++;
4299 }
4300
4301 if (body->Index > 0.0) {
4302 ancestorIndices1->data[static_cast<int32_T>
4303 (cartesian_trajectory_planner_B.bid1) - 1] = body->ParentIndex;
4304 cartesian_trajectory_planner_B.bid1++;
4305 }
4306
4307 cartesian_trajec_emxInit_real_T(&ancestorIndices1_0, 2);
4308 cartesian_trajectory_planner_B.loop_ub_i = static_cast<int32_T>
4309 (cartesian_trajectory_planner_B.bid1 - 1.0);
4310 cartesian_trajectory_planner_B.b_kstr = ancestorIndices1_0->size[0] *
4311 ancestorIndices1_0->size[1];
4312 ancestorIndices1_0->size[0] = 1;
4313 ancestorIndices1_0->size[1] = cartesian_trajectory_planner_B.loop_ub_i;
4314 cartes_emxEnsureCapacity_real_T(ancestorIndices1_0,
4315 cartesian_trajectory_planner_B.b_kstr);
4316 for (cartesian_trajectory_planner_B.b_kstr = 0;
4317 cartesian_trajectory_planner_B.b_kstr <
4318 cartesian_trajectory_planner_B.loop_ub_i;
4319 cartesian_trajectory_planner_B.b_kstr++) {
4320 ancestorIndices1_0->data[cartesian_trajectory_planner_B.b_kstr] =
4321 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr];
4322 }
4323
4324 cartesian_trajectory_planner_B.b_kstr = ancestorIndices1->size[0] *
4325 ancestorIndices1->size[1];
4326 ancestorIndices1->size[0] = 1;
4327 ancestorIndices1->size[1] = ancestorIndices1_0->size[1];
4328 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
4329 cartesian_trajectory_planner_B.b_kstr);
4330 cartesian_trajectory_planner_B.loop_ub_i = ancestorIndices1_0->size[0] *
4331 ancestorIndices1_0->size[1];
4332 for (cartesian_trajectory_planner_B.b_kstr = 0;
4333 cartesian_trajectory_planner_B.b_kstr <
4334 cartesian_trajectory_planner_B.loop_ub_i;
4335 cartesian_trajectory_planner_B.b_kstr++) {
4336 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr] =
4337 ancestorIndices1_0->data[cartesian_trajectory_planner_B.b_kstr];
4338 }
4339
4340 cartesian_trajec_emxFree_real_T(&ancestorIndices1_0);
4341 cartesian_trajec_emxInit_real_T(&ancestorIndices2, 2);
4342 body = body2;
4343 cartesian_trajectory_planner_B.b_kstr = ancestorIndices2->size[0] *
4344 ancestorIndices2->size[1];
4345 ancestorIndices2->size[0] = 1;
4346 ancestorIndices2->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
4347 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
4348 cartesian_trajectory_planner_B.b_kstr);
4349 cartesian_trajectory_planner_B.loop_ub_i = static_cast<int32_T>(obj->NumBodies
4350 + 1.0) - 1;
4351 for (cartesian_trajectory_planner_B.b_kstr = 0;
4352 cartesian_trajectory_planner_B.b_kstr <=
4353 cartesian_trajectory_planner_B.loop_ub_i;
4354 cartesian_trajectory_planner_B.b_kstr++) {
4355 ancestorIndices2->data[cartesian_trajectory_planner_B.b_kstr] = 0.0;
4356 }
4357
4358 cartesian_trajectory_planner_B.bid1 = 2.0;
4359 ancestorIndices2->data[0] = body2->Index;
4360 while (body->ParentIndex > 0.0) {
4361 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
4362 ancestorIndices2->data[static_cast<int32_T>
4363 (cartesian_trajectory_planner_B.bid1) - 1] = body->Index;
4364 cartesian_trajectory_planner_B.bid1++;
4365 }
4366
4367 if (body->Index > 0.0) {
4368 ancestorIndices2->data[static_cast<int32_T>
4369 (cartesian_trajectory_planner_B.bid1) - 1] = body->ParentIndex;
4370 cartesian_trajectory_planner_B.bid1++;
4371 }
4372
4373 cartesian_trajec_emxInit_real_T(&ancestorIndices2_0, 2);
4374 cartesian_trajectory_planner_B.loop_ub_i = static_cast<int32_T>
4375 (cartesian_trajectory_planner_B.bid1 - 1.0);
4376 cartesian_trajectory_planner_B.b_kstr = ancestorIndices2_0->size[0] *
4377 ancestorIndices2_0->size[1];
4378 ancestorIndices2_0->size[0] = 1;
4379 ancestorIndices2_0->size[1] = cartesian_trajectory_planner_B.loop_ub_i;
4380 cartes_emxEnsureCapacity_real_T(ancestorIndices2_0,
4381 cartesian_trajectory_planner_B.b_kstr);
4382 for (cartesian_trajectory_planner_B.b_kstr = 0;
4383 cartesian_trajectory_planner_B.b_kstr <
4384 cartesian_trajectory_planner_B.loop_ub_i;
4385 cartesian_trajectory_planner_B.b_kstr++) {
4386 ancestorIndices2_0->data[cartesian_trajectory_planner_B.b_kstr] =
4387 ancestorIndices2->data[cartesian_trajectory_planner_B.b_kstr];
4388 }
4389
4390 cartesian_trajectory_planner_B.b_kstr = ancestorIndices2->size[0] *
4391 ancestorIndices2->size[1];
4392 ancestorIndices2->size[0] = 1;
4393 ancestorIndices2->size[1] = ancestorIndices2_0->size[1];
4394 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
4395 cartesian_trajectory_planner_B.b_kstr);
4396 cartesian_trajectory_planner_B.loop_ub_i = ancestorIndices2_0->size[0] *
4397 ancestorIndices2_0->size[1];
4398 for (cartesian_trajectory_planner_B.b_kstr = 0;
4399 cartesian_trajectory_planner_B.b_kstr <
4400 cartesian_trajectory_planner_B.loop_ub_i;
4401 cartesian_trajectory_planner_B.b_kstr++) {
4402 ancestorIndices2->data[cartesian_trajectory_planner_B.b_kstr] =
4403 ancestorIndices2_0->data[cartesian_trajectory_planner_B.b_kstr];
4404 }
4405
4406 cartesian_trajec_emxFree_real_T(&ancestorIndices2_0);
4407 cartesian_trajectory_planner_B.bid1 = ancestorIndices1->size[1];
4408 cartesian_trajectory_planner_B.qidx_idx_1 = ancestorIndices2->size[1];
4409 if (cartesian_trajectory_planner_B.bid1 <
4410 cartesian_trajectory_planner_B.qidx_idx_1) {
4411 cartesian_trajectory_planner_B.qidx_idx_1 =
4412 cartesian_trajectory_planner_B.bid1;
4413 }
4414
4415 cartesian_trajectory_planner_B.bid1 = static_cast<int32_T>
4416 (cartesian_trajectory_planner_B.qidx_idx_1);
4417 cartesian_trajectory_planner_B.b_i_c = 0;
4418 exitg1 = false;
4419 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_c <=
4420 static_cast<int32_T>
4421 (cartesian_trajectory_planner_B.qidx_idx_1) - 2)) {
4422 if (ancestorIndices1->data[static_cast<int32_T>(static_cast<real_T>
4423 (ancestorIndices1->size[1]) - (static_cast<real_T>
4424 (cartesian_trajectory_planner_B.b_i_c) + 1.0)) - 1] !=
4425 ancestorIndices2->data[static_cast<int32_T>(static_cast<real_T>
4426 (ancestorIndices2->size[1]) - (static_cast<real_T>
4427 (cartesian_trajectory_planner_B.b_i_c) + 1.0)) - 1]) {
4428 cartesian_trajectory_planner_B.bid1 = static_cast<real_T>
4429 (cartesian_trajectory_planner_B.b_i_c) + 1.0;
4430 exitg1 = true;
4431 } else {
4432 cartesian_trajectory_planner_B.b_i_c++;
4433 }
4434 }
4435
4436 cartesian_trajectory_planner_B.qidx_idx_1 = static_cast<real_T>
4437 (ancestorIndices1->size[1]) - cartesian_trajectory_planner_B.bid1;
4438 if (1.0 > cartesian_trajectory_planner_B.qidx_idx_1) {
4439 cartesian_trajectory_planner_B.b_i_c = -1;
4440 } else {
4441 cartesian_trajectory_planner_B.b_i_c = static_cast<int32_T>
4442 (cartesian_trajectory_planner_B.qidx_idx_1) - 1;
4443 }
4444
4445 cartesian_trajectory_planner_B.qidx_idx_1 = static_cast<real_T>
4446 (ancestorIndices2->size[1]) - cartesian_trajectory_planner_B.bid1;
4447 if (1.0 > cartesian_trajectory_planner_B.qidx_idx_1) {
4448 cartesian_trajectory_planner_B.j_o = 0;
4449 cartesian_trajectory_planner_B.jointSign = 1;
4450 cartesian_trajectory_planner_B.g = -1;
4451 } else {
4452 cartesian_trajectory_planner_B.j_o = static_cast<int32_T>
4453 (cartesian_trajectory_planner_B.qidx_idx_1) - 1;
4454 cartesian_trajectory_planner_B.jointSign = -1;
4455 cartesian_trajectory_planner_B.g = 0;
4456 }
4457
4458 cartesian_trajec_emxInit_real_T(&kinematicPathIndices, 2);
4459 cartesian_trajectory_planner_B.b_kstr = kinematicPathIndices->size[0] *
4460 kinematicPathIndices->size[1];
4461 kinematicPathIndices->size[0] = 1;
4462 cartesian_trajectory_planner_B.loop_ub_i = div_s32_floor
4463 (cartesian_trajectory_planner_B.g - cartesian_trajectory_planner_B.j_o,
4464 cartesian_trajectory_planner_B.jointSign);
4465 kinematicPathIndices->size[1] = (cartesian_trajectory_planner_B.loop_ub_i +
4466 cartesian_trajectory_planner_B.b_i_c) + 3;
4467 cartes_emxEnsureCapacity_real_T(kinematicPathIndices,
4468 cartesian_trajectory_planner_B.b_kstr);
4469 for (cartesian_trajectory_planner_B.b_kstr = 0;
4470 cartesian_trajectory_planner_B.b_kstr <=
4471 cartesian_trajectory_planner_B.b_i_c;
4472 cartesian_trajectory_planner_B.b_kstr++) {
4473 kinematicPathIndices->data[cartesian_trajectory_planner_B.b_kstr] =
4474 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr];
4475 }
4476
4477 kinematicPathIndices->data[cartesian_trajectory_planner_B.b_i_c + 1] =
4478 ancestorIndices1->data[static_cast<int32_T>((static_cast<real_T>
4479 (ancestorIndices1->size[1]) - cartesian_trajectory_planner_B.bid1) + 1.0) -
4480 1];
4481 cartesian_trajec_emxFree_real_T(&ancestorIndices1);
4482 for (cartesian_trajectory_planner_B.b_kstr = 0;
4483 cartesian_trajectory_planner_B.b_kstr <=
4484 cartesian_trajectory_planner_B.loop_ub_i;
4485 cartesian_trajectory_planner_B.b_kstr++) {
4486 kinematicPathIndices->data[(cartesian_trajectory_planner_B.b_kstr +
4487 cartesian_trajectory_planner_B.b_i_c) + 2] = ancestorIndices2->
4488 data[cartesian_trajectory_planner_B.jointSign *
4489 cartesian_trajectory_planner_B.b_kstr + cartesian_trajectory_planner_B.j_o];
4490 }
4491
4492 cartesian_trajec_emxFree_real_T(&ancestorIndices2);
4493 memset(&cartesian_trajectory_planner_B.T1[0], 0, sizeof(real_T) << 4U);
4494 cartesian_trajectory_planner_B.T1[0] = 1.0;
4495 cartesian_trajectory_planner_B.T1[5] = 1.0;
4496 cartesian_trajectory_planner_B.T1[10] = 1.0;
4497 cartesian_trajectory_planner_B.T1[15] = 1.0;
4498 cartesian_trajectory_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
4499 Jac->size[0] = 6;
4500 Jac->size[1] = static_cast<int32_T>(obj->PositionNumber);
4501 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr);
4502 cartesian_trajectory_planner_B.loop_ub_i = 6 * static_cast<int32_T>
4503 (obj->PositionNumber) - 1;
4504 for (cartesian_trajectory_planner_B.b_kstr = 0;
4505 cartesian_trajectory_planner_B.b_kstr <=
4506 cartesian_trajectory_planner_B.loop_ub_i;
4507 cartesian_trajectory_planner_B.b_kstr++) {
4508 Jac->data[cartesian_trajectory_planner_B.b_kstr] = 0.0;
4509 }
4510
4511 cartesian_trajectory_planner_B.j_o = kinematicPathIndices->size[1] - 2;
4512 cartesian_trajec_emxInit_real_T(&y, 2);
4513 cartesian_trajec_emxInit_real_T(&b, 2);
4514 if (0 <= cartesian_trajectory_planner_B.j_o) {
4515 for (cartesian_trajectory_planner_B.b_kstr = 0;
4516 cartesian_trajectory_planner_B.b_kstr < 5;
4517 cartesian_trajectory_planner_B.b_kstr++) {
4518 cartesian_trajectory_planner_B.b_fa[cartesian_trajectory_planner_B.b_kstr]
4519 = tmp[cartesian_trajectory_planner_B.b_kstr];
4520 }
4521 }
4522
4523 for (cartesian_trajectory_planner_B.b_i_c = 0;
4524 cartesian_trajectory_planner_B.b_i_c <=
4525 cartesian_trajectory_planner_B.j_o; cartesian_trajectory_planner_B.b_i_c
4526 ++) {
4527 if (kinematicPathIndices->data[cartesian_trajectory_planner_B.b_i_c] != 0.0)
4528 {
4529 body1 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
4530 data[cartesian_trajectory_planner_B.b_i_c]) - 1];
4531 } else {
4532 body1 = &obj->Base;
4533 }
4534
4535 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>
4536 ((static_cast<real_T>(cartesian_trajectory_planner_B.b_i_c) + 1.0) + 1.0)
4537 - 1;
4538 if (kinematicPathIndices->data[cartesian_trajectory_planner_B.b_kstr] != 0.0)
4539 {
4540 body2 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
4541 data[cartesian_trajectory_planner_B.b_kstr]) - 1];
4542 } else {
4543 body2 = &obj->Base;
4544 }
4545
4546 cartesian_trajectory_planner_B.nextBodyIsParent = (body2->Index ==
4547 body1->ParentIndex);
4548 if (cartesian_trajectory_planner_B.nextBodyIsParent) {
4549 body2 = body1;
4550 cartesian_trajectory_planner_B.jointSign = 1;
4551 } else {
4552 cartesian_trajectory_planner_B.jointSign = -1;
4553 }
4554
4555 joint = body2->JointInternal;
4556 cartesian_trajectory_planner_B.b_kstr = body2Name->size[0] * body2Name->
4557 size[1];
4558 body2Name->size[0] = 1;
4559 body2Name->size[1] = joint->Type->size[1];
4560 cart_emxEnsureCapacity_char_T_a(body2Name,
4561 cartesian_trajectory_planner_B.b_kstr);
4562 cartesian_trajectory_planner_B.loop_ub_i = joint->Type->size[0] *
4563 joint->Type->size[1] - 1;
4564 for (cartesian_trajectory_planner_B.b_kstr = 0;
4565 cartesian_trajectory_planner_B.b_kstr <=
4566 cartesian_trajectory_planner_B.loop_ub_i;
4567 cartesian_trajectory_planner_B.b_kstr++) {
4568 body2Name->data[cartesian_trajectory_planner_B.b_kstr] = joint->Type->
4569 data[cartesian_trajectory_planner_B.b_kstr];
4570 }
4571
4572 cartesian_trajectory_planner_B.b_bool = false;
4573 if (body2Name->size[1] == 5) {
4574 cartesian_trajectory_planner_B.b_kstr = 1;
4575 do {
4576 exitg2 = 0;
4577 if (cartesian_trajectory_planner_B.b_kstr - 1 < 5) {
4578 cartesian_trajectory_planner_B.g =
4579 cartesian_trajectory_planner_B.b_kstr - 1;
4580 if (body2Name->data[cartesian_trajectory_planner_B.g] !=
4581 cartesian_trajectory_planner_B.b_fa[cartesian_trajectory_planner_B.g])
4582 {
4583 exitg2 = 1;
4584 } else {
4585 cartesian_trajectory_planner_B.b_kstr++;
4586 }
4587 } else {
4588 cartesian_trajectory_planner_B.b_bool = true;
4589 exitg2 = 1;
4590 }
4591 } while (exitg2 == 0);
4592 }
4593
4594 if (cartesian_trajectory_planner_B.b_bool) {
4595 rigidBodyJoint_transformBodyToP(joint, cartesian_trajectory_planner_B.Tc2p);
4596 } else {
4597 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
4598 cartesian_trajectory_planner_B.bid1 = obj->
4599 PositionDoFMap[cartesian_trajectory_planner_B.b_kstr - 1];
4600 cartesian_trajectory_planner_B.qidx_idx_1 = obj->
4601 PositionDoFMap[cartesian_trajectory_planner_B.b_kstr + 7];
4602 if (cartesian_trajectory_planner_B.bid1 >
4603 cartesian_trajectory_planner_B.qidx_idx_1) {
4604 cartesian_trajectory_planner_B.g = 0;
4605 cartesian_trajectory_planner_B.b_kstr = -1;
4606 } else {
4607 cartesian_trajectory_planner_B.g = static_cast<int32_T>
4608 (cartesian_trajectory_planner_B.bid1) - 1;
4609 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>
4610 (cartesian_trajectory_planner_B.qidx_idx_1) - 1;
4611 }
4612
4613 cartesian_trajectory_planner_B.loop_ub_i =
4614 cartesian_trajectory_planner_B.b_kstr - cartesian_trajectory_planner_B.g;
4615 cartesian_trajectory_planner_B.qv_size =
4616 cartesian_trajectory_planner_B.loop_ub_i + 1;
4617 for (cartesian_trajectory_planner_B.b_kstr = 0;
4618 cartesian_trajectory_planner_B.b_kstr <=
4619 cartesian_trajectory_planner_B.loop_ub_i;
4620 cartesian_trajectory_planner_B.b_kstr++) {
4621 cartesian_trajectory_planner_B.qv_data[cartesian_trajectory_planner_B.b_kstr]
4622 = qv[cartesian_trajectory_planner_B.g +
4623 cartesian_trajectory_planner_B.b_kstr];
4624 }
4625
4626 rigidBodyJoint_transformBodyT_a(joint,
4627 cartesian_trajectory_planner_B.qv_data,
4628 &cartesian_trajectory_planner_B.qv_size,
4629 cartesian_trajectory_planner_B.Tc2p);
4630 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
4631 cartesian_trajectory_planner_B.bid1 = obj->
4632 VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr - 1];
4633 cartesian_trajectory_planner_B.qidx_idx_1 = obj->
4634 VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr + 7];
4635 if (cartesian_trajectory_planner_B.nextBodyIsParent) {
4636 for (cartesian_trajectory_planner_B.b_kstr = 0;
4637 cartesian_trajectory_planner_B.b_kstr < 16;
4638 cartesian_trajectory_planner_B.b_kstr++) {
4639 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr]
4640 = joint->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr];
4641 }
4642 } else {
4643 for (cartesian_trajectory_planner_B.b_kstr = 0;
4644 cartesian_trajectory_planner_B.b_kstr < 16;
4645 cartesian_trajectory_planner_B.b_kstr++) {
4646 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr]
4647 = joint->
4648 JointToParentTransform[cartesian_trajectory_planner_B.b_kstr];
4649 }
4650
4651 for (cartesian_trajectory_planner_B.b_kstr = 0;
4652 cartesian_trajectory_planner_B.b_kstr < 3;
4653 cartesian_trajectory_planner_B.b_kstr++) {
4654 cartesian_trajectory_planner_B.R_o[3 *
4655 cartesian_trajectory_planner_B.b_kstr] =
4656 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr];
4657 cartesian_trajectory_planner_B.R_o[3 *
4658 cartesian_trajectory_planner_B.b_kstr + 1] =
4659 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
4660 + 4];
4661 cartesian_trajectory_planner_B.R_o[3 *
4662 cartesian_trajectory_planner_B.b_kstr + 2] =
4663 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
4664 + 8];
4665 }
4666
4667 for (cartesian_trajectory_planner_B.b_kstr = 0;
4668 cartesian_trajectory_planner_B.b_kstr < 9;
4669 cartesian_trajectory_planner_B.b_kstr++) {
4670 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr]
4671 =
4672 -cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr];
4673 }
4674
4675 for (cartesian_trajectory_planner_B.b_kstr = 0;
4676 cartesian_trajectory_planner_B.b_kstr < 3;
4677 cartesian_trajectory_planner_B.b_kstr++) {
4678 cartesian_trajectory_planner_B.g =
4679 cartesian_trajectory_planner_B.b_kstr << 2;
4680 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g] =
4681 cartesian_trajectory_planner_B.R_o[3 *
4682 cartesian_trajectory_planner_B.b_kstr];
4683 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 1]
4684 = cartesian_trajectory_planner_B.R_o[3 *
4685 cartesian_trajectory_planner_B.b_kstr + 1];
4686 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 2]
4687 = cartesian_trajectory_planner_B.R_o[3 *
4688 cartesian_trajectory_planner_B.b_kstr + 2];
4689 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
4690 + 12] =
4691 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr
4692 + 6] * cartesian_trajectory_planner_B.T1j[14] +
4693 (cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr
4694 + 3] * cartesian_trajectory_planner_B.T1j[13] +
4695 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr]
4696 * cartesian_trajectory_planner_B.T1j[12]);
4697 }
4698
4699 cartesian_trajectory_planner_B.Tj[3] = 0.0;
4700 cartesian_trajectory_planner_B.Tj[7] = 0.0;
4701 cartesian_trajectory_planner_B.Tj[11] = 0.0;
4702 cartesian_trajectory_planner_B.Tj[15] = 1.0;
4703 }
4704
4705 for (cartesian_trajectory_planner_B.b_kstr = 0;
4706 cartesian_trajectory_planner_B.b_kstr < 4;
4707 cartesian_trajectory_planner_B.b_kstr++) {
4708 for (cartesian_trajectory_planner_B.g = 0;
4709 cartesian_trajectory_planner_B.g < 4;
4710 cartesian_trajectory_planner_B.g++) {
4711 cartesian_trajectory_planner_B.loop_ub_i =
4712 cartesian_trajectory_planner_B.g << 2;
4713 cartesian_trajectory_planner_B.n_h =
4714 cartesian_trajectory_planner_B.b_kstr +
4715 cartesian_trajectory_planner_B.loop_ub_i;
4716 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_h]
4717 = 0.0;
4718 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_h]
4719 +=
4720 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_i]
4721 * cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr];
4722 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_h]
4723 +=
4724 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_i
4725 + 1] *
4726 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
4727 + 4];
4728 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_h]
4729 +=
4730 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_i
4731 + 2] *
4732 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
4733 + 8];
4734 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_h]
4735 +=
4736 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_i
4737 + 3] *
4738 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
4739 + 12];
4740 }
4741 }
4742
4743 for (cartesian_trajectory_planner_B.b_kstr = 0;
4744 cartesian_trajectory_planner_B.b_kstr < 3;
4745 cartesian_trajectory_planner_B.b_kstr++) {
4746 cartesian_trajectory_planner_B.R_o[3 *
4747 cartesian_trajectory_planner_B.b_kstr] =
4748 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr];
4749 cartesian_trajectory_planner_B.R_o[3 *
4750 cartesian_trajectory_planner_B.b_kstr + 1] =
4751 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
4752 + 4];
4753 cartesian_trajectory_planner_B.R_o[3 *
4754 cartesian_trajectory_planner_B.b_kstr + 2] =
4755 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
4756 + 8];
4757 }
4758
4759 for (cartesian_trajectory_planner_B.b_kstr = 0;
4760 cartesian_trajectory_planner_B.b_kstr < 9;
4761 cartesian_trajectory_planner_B.b_kstr++) {
4762 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr]
4763 =
4764 -cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr];
4765 }
4766
4767 for (cartesian_trajectory_planner_B.b_kstr = 0;
4768 cartesian_trajectory_planner_B.b_kstr < 3;
4769 cartesian_trajectory_planner_B.b_kstr++) {
4770 cartesian_trajectory_planner_B.g = cartesian_trajectory_planner_B.b_kstr
4771 << 2;
4772 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g] =
4773 cartesian_trajectory_planner_B.R_o[3 *
4774 cartesian_trajectory_planner_B.b_kstr];
4775 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 1] =
4776 cartesian_trajectory_planner_B.R_o[3 *
4777 cartesian_trajectory_planner_B.b_kstr + 1];
4778 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 2] =
4779 cartesian_trajectory_planner_B.R_o[3 *
4780 cartesian_trajectory_planner_B.b_kstr + 2];
4781 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
4782 + 12] =
4783 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr
4784 + 6] * cartesian_trajectory_planner_B.T1j[14] +
4785 (cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr
4786 + 3] * cartesian_trajectory_planner_B.T1j[13] +
4787 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr]
4788 * cartesian_trajectory_planner_B.T1j[12]);
4789 }
4790
4791 cartesian_trajectory_planner_B.Tj[3] = 0.0;
4792 cartesian_trajectory_planner_B.Tj[7] = 0.0;
4793 cartesian_trajectory_planner_B.Tj[11] = 0.0;
4794 cartesian_trajectory_planner_B.Tj[15] = 1.0;
4795 cartesian_trajectory_planner_B.R_o[0] = 0.0;
4796 cartesian_trajectory_planner_B.R_o[3] =
4797 -cartesian_trajectory_planner_B.Tj[14];
4798 cartesian_trajectory_planner_B.R_o[6] = cartesian_trajectory_planner_B.Tj
4799 [13];
4800 cartesian_trajectory_planner_B.R_o[1] = cartesian_trajectory_planner_B.Tj
4801 [14];
4802 cartesian_trajectory_planner_B.R_o[4] = 0.0;
4803 cartesian_trajectory_planner_B.R_o[7] =
4804 -cartesian_trajectory_planner_B.Tj[12];
4805 cartesian_trajectory_planner_B.R_o[2] =
4806 -cartesian_trajectory_planner_B.Tj[13];
4807 cartesian_trajectory_planner_B.R_o[5] = cartesian_trajectory_planner_B.Tj
4808 [12];
4809 cartesian_trajectory_planner_B.R_o[8] = 0.0;
4810 for (cartesian_trajectory_planner_B.b_kstr = 0;
4811 cartesian_trajectory_planner_B.b_kstr < 3;
4812 cartesian_trajectory_planner_B.b_kstr++) {
4813 for (cartesian_trajectory_planner_B.g = 0;
4814 cartesian_trajectory_planner_B.g < 3;
4815 cartesian_trajectory_planner_B.g++) {
4816 cartesian_trajectory_planner_B.loop_ub_i =
4817 cartesian_trajectory_planner_B.b_kstr + 3 *
4818 cartesian_trajectory_planner_B.g;
4819 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.loop_ub_i]
4820 = 0.0;
4821 cartesian_trajectory_planner_B.n_h = cartesian_trajectory_planner_B.g <<
4822 2;
4823 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.loop_ub_i]
4824 +=
4825 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.n_h]
4826 * cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr];
4827 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.loop_ub_i]
4828 +=
4829 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.n_h
4830 + 1] *
4831 cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr
4832 + 3];
4833 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.loop_ub_i]
4834 +=
4835 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.n_h
4836 + 2] *
4837 cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr
4838 + 6];
4839 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 6 *
4840 cartesian_trajectory_planner_B.b_kstr] =
4841 cartesian_trajectory_planner_B.Tj
4842 [(cartesian_trajectory_planner_B.b_kstr << 2) +
4843 cartesian_trajectory_planner_B.g];
4844 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 6 *
4845 (cartesian_trajectory_planner_B.b_kstr + 3)] = 0.0;
4846 }
4847 }
4848
4849 for (cartesian_trajectory_planner_B.b_kstr = 0;
4850 cartesian_trajectory_planner_B.b_kstr < 3;
4851 cartesian_trajectory_planner_B.b_kstr++) {
4852 cartesian_trajectory_planner_B.X[6 *
4853 cartesian_trajectory_planner_B.b_kstr + 3] =
4854 cartesian_trajectory_planner_B.R_i[3 *
4855 cartesian_trajectory_planner_B.b_kstr];
4856 cartesian_trajectory_planner_B.g = cartesian_trajectory_planner_B.b_kstr
4857 << 2;
4858 cartesian_trajectory_planner_B.loop_ub_i = 6 *
4859 (cartesian_trajectory_planner_B.b_kstr + 3);
4860 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_i
4861 + 3] =
4862 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g];
4863 cartesian_trajectory_planner_B.X[6 *
4864 cartesian_trajectory_planner_B.b_kstr + 4] =
4865 cartesian_trajectory_planner_B.R_i[3 *
4866 cartesian_trajectory_planner_B.b_kstr + 1];
4867 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_i
4868 + 4] =
4869 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 1];
4870 cartesian_trajectory_planner_B.X[6 *
4871 cartesian_trajectory_planner_B.b_kstr + 5] =
4872 cartesian_trajectory_planner_B.R_i[3 *
4873 cartesian_trajectory_planner_B.b_kstr + 2];
4874 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_i
4875 + 5] =
4876 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 2];
4877 }
4878
4879 cartesian_trajectory_planner_B.b_kstr = b->size[0] * b->size[1];
4880 b->size[0] = 6;
4881 b->size[1] = joint->MotionSubspace->size[1];
4882 cartes_emxEnsureCapacity_real_T(b, cartesian_trajectory_planner_B.b_kstr);
4883 cartesian_trajectory_planner_B.loop_ub_i = joint->MotionSubspace->size[0] *
4884 joint->MotionSubspace->size[1] - 1;
4885 for (cartesian_trajectory_planner_B.b_kstr = 0;
4886 cartesian_trajectory_planner_B.b_kstr <=
4887 cartesian_trajectory_planner_B.loop_ub_i;
4888 cartesian_trajectory_planner_B.b_kstr++) {
4889 b->data[cartesian_trajectory_planner_B.b_kstr] = joint->
4890 MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr];
4891 }
4892
4893 cartesian_trajectory_planner_B.n_h = b->size[1] - 1;
4894 cartesian_trajectory_planner_B.b_kstr = y->size[0] * y->size[1];
4895 y->size[0] = 6;
4896 y->size[1] = b->size[1];
4897 cartes_emxEnsureCapacity_real_T(y, cartesian_trajectory_planner_B.b_kstr);
4898 for (cartesian_trajectory_planner_B.b_kstr = 0;
4899 cartesian_trajectory_planner_B.b_kstr <=
4900 cartesian_trajectory_planner_B.n_h;
4901 cartesian_trajectory_planner_B.b_kstr++) {
4902 cartesian_trajectory_planner_B.coffset_tmp =
4903 cartesian_trajectory_planner_B.b_kstr * 6 - 1;
4904 for (cartesian_trajectory_planner_B.g = 0;
4905 cartesian_trajectory_planner_B.g < 6;
4906 cartesian_trajectory_planner_B.g++) {
4907 cartesian_trajectory_planner_B.bid2 = 0.0;
4908 for (cartesian_trajectory_planner_B.loop_ub_i = 0;
4909 cartesian_trajectory_planner_B.loop_ub_i < 6;
4910 cartesian_trajectory_planner_B.loop_ub_i++) {
4911 cartesian_trajectory_planner_B.bid2 +=
4912 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_i
4913 * 6 + cartesian_trajectory_planner_B.g] * b->data
4914 [(cartesian_trajectory_planner_B.coffset_tmp +
4915 cartesian_trajectory_planner_B.loop_ub_i) + 1];
4916 }
4917
4918 y->data[(cartesian_trajectory_planner_B.coffset_tmp +
4919 cartesian_trajectory_planner_B.g) + 1] =
4920 cartesian_trajectory_planner_B.bid2;
4921 }
4922 }
4923
4924 if (cartesian_trajectory_planner_B.bid1 >
4925 cartesian_trajectory_planner_B.qidx_idx_1) {
4926 cartesian_trajectory_planner_B.n_h = 0;
4927 } else {
4928 cartesian_trajectory_planner_B.n_h = static_cast<int32_T>
4929 (cartesian_trajectory_planner_B.bid1) - 1;
4930 }
4931
4932 cartesian_trajectory_planner_B.loop_ub_i = y->size[1];
4933 for (cartesian_trajectory_planner_B.b_kstr = 0;
4934 cartesian_trajectory_planner_B.b_kstr <
4935 cartesian_trajectory_planner_B.loop_ub_i;
4936 cartesian_trajectory_planner_B.b_kstr++) {
4937 for (cartesian_trajectory_planner_B.g = 0;
4938 cartesian_trajectory_planner_B.g < 6;
4939 cartesian_trajectory_planner_B.g++) {
4940 Jac->data[cartesian_trajectory_planner_B.g + 6 *
4941 (cartesian_trajectory_planner_B.n_h +
4942 cartesian_trajectory_planner_B.b_kstr)] = y->data[6 *
4943 cartesian_trajectory_planner_B.b_kstr +
4944 cartesian_trajectory_planner_B.g] * static_cast<real_T>
4945 (cartesian_trajectory_planner_B.jointSign);
4946 }
4947 }
4948 }
4949
4950 if (cartesian_trajectory_planner_B.nextBodyIsParent) {
4951 for (cartesian_trajectory_planner_B.b_kstr = 0;
4952 cartesian_trajectory_planner_B.b_kstr < 4;
4953 cartesian_trajectory_planner_B.b_kstr++) {
4954 for (cartesian_trajectory_planner_B.g = 0;
4955 cartesian_trajectory_planner_B.g < 4;
4956 cartesian_trajectory_planner_B.g++) {
4957 cartesian_trajectory_planner_B.loop_ub_i =
4958 cartesian_trajectory_planner_B.g << 2;
4959 cartesian_trajectory_planner_B.jointSign =
4960 cartesian_trajectory_planner_B.b_kstr +
4961 cartesian_trajectory_planner_B.loop_ub_i;
4962 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
4963 = 0.0;
4964 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
4965 +=
4966 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_i]
4967 * cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr];
4968 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
4969 +=
4970 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_i
4971 + 1] *
4972 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
4973 + 4];
4974 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
4975 +=
4976 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_i
4977 + 2] *
4978 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
4979 + 8];
4980 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
4981 +=
4982 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_i
4983 + 3] *
4984 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
4985 + 12];
4986 }
4987 }
4988
4989 memcpy(&cartesian_trajectory_planner_B.T1[0],
4990 &cartesian_trajectory_planner_B.T1j[0], sizeof(real_T) << 4U);
4991 } else {
4992 for (cartesian_trajectory_planner_B.b_kstr = 0;
4993 cartesian_trajectory_planner_B.b_kstr < 3;
4994 cartesian_trajectory_planner_B.b_kstr++) {
4995 cartesian_trajectory_planner_B.R_o[3 *
4996 cartesian_trajectory_planner_B.b_kstr] =
4997 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr];
4998 cartesian_trajectory_planner_B.R_o[3 *
4999 cartesian_trajectory_planner_B.b_kstr + 1] =
5000 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
5001 + 4];
5002 cartesian_trajectory_planner_B.R_o[3 *
5003 cartesian_trajectory_planner_B.b_kstr + 2] =
5004 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
5005 + 8];
5006 }
5007
5008 for (cartesian_trajectory_planner_B.b_kstr = 0;
5009 cartesian_trajectory_planner_B.b_kstr < 9;
5010 cartesian_trajectory_planner_B.b_kstr++) {
5011 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr]
5012 =
5013 -cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr];
5014 }
5015
5016 for (cartesian_trajectory_planner_B.b_kstr = 0;
5017 cartesian_trajectory_planner_B.b_kstr < 3;
5018 cartesian_trajectory_planner_B.b_kstr++) {
5019 cartesian_trajectory_planner_B.loop_ub_i =
5020 cartesian_trajectory_planner_B.b_kstr << 2;
5021 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.loop_ub_i]
5022 = cartesian_trajectory_planner_B.R_o[3 *
5023 cartesian_trajectory_planner_B.b_kstr];
5024 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.loop_ub_i
5025 + 1] = cartesian_trajectory_planner_B.R_o[3 *
5026 cartesian_trajectory_planner_B.b_kstr + 1];
5027 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.loop_ub_i
5028 + 2] = cartesian_trajectory_planner_B.R_o[3 *
5029 cartesian_trajectory_planner_B.b_kstr + 2];
5030 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
5031 + 12] =
5032 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr
5033 + 6] * cartesian_trajectory_planner_B.Tc2p[14] +
5034 (cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr
5035 + 3] * cartesian_trajectory_planner_B.Tc2p[13] +
5036 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr]
5037 * cartesian_trajectory_planner_B.Tc2p[12]);
5038 }
5039
5040 cartesian_trajectory_planner_B.T1j[3] = 0.0;
5041 cartesian_trajectory_planner_B.T1j[7] = 0.0;
5042 cartesian_trajectory_planner_B.T1j[11] = 0.0;
5043 cartesian_trajectory_planner_B.T1j[15] = 1.0;
5044 for (cartesian_trajectory_planner_B.b_kstr = 0;
5045 cartesian_trajectory_planner_B.b_kstr < 4;
5046 cartesian_trajectory_planner_B.b_kstr++) {
5047 for (cartesian_trajectory_planner_B.g = 0;
5048 cartesian_trajectory_planner_B.g < 4;
5049 cartesian_trajectory_planner_B.g++) {
5050 cartesian_trajectory_planner_B.jointSign =
5051 cartesian_trajectory_planner_B.g << 2;
5052 cartesian_trajectory_planner_B.loop_ub_i =
5053 cartesian_trajectory_planner_B.b_kstr +
5054 cartesian_trajectory_planner_B.jointSign;
5055 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_i]
5056 = 0.0;
5057 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_i]
5058 +=
5059 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign]
5060 * cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr];
5061 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_i]
5062 +=
5063 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign
5064 + 1] *
5065 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
5066 + 4];
5067 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_i]
5068 +=
5069 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign
5070 + 2] *
5071 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
5072 + 8];
5073 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_i]
5074 +=
5075 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign
5076 + 3] *
5077 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
5078 + 12];
5079 }
5080 }
5081
5082 memcpy(&cartesian_trajectory_planner_B.T1[0],
5083 &cartesian_trajectory_planner_B.Tc2p[0], sizeof(real_T) << 4U);
5084 }
5085 }
5086
5087 cartesian_trajec_emxFree_real_T(&b);
5088 cartesian_traj_emxFree_char_T_a(&body2Name);
5089 cartesian_trajec_emxFree_real_T(&kinematicPathIndices);
5090 for (cartesian_trajectory_planner_B.b_kstr = 0;
5091 cartesian_trajectory_planner_B.b_kstr < 3;
5092 cartesian_trajectory_planner_B.b_kstr++) {
5093 cartesian_trajectory_planner_B.b_i_c = cartesian_trajectory_planner_B.b_kstr
5094 << 2;
5095 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr] =
5096 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_c];
5097 cartesian_trajectory_planner_B.g = 6 *
5098 (cartesian_trajectory_planner_B.b_kstr + 3);
5099 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g] = 0.0;
5100 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
5101 3] = 0.0;
5102 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 3] =
5103 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_c];
5104 cartesian_trajectory_planner_B.bid1 =
5105 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_c + 1];
5106 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
5107 1] = cartesian_trajectory_planner_B.bid1;
5108 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 1] = 0.0;
5109 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
5110 4] = 0.0;
5111 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 4] =
5112 cartesian_trajectory_planner_B.bid1;
5113 cartesian_trajectory_planner_B.bid1 =
5114 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_c + 2];
5115 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
5116 2] = cartesian_trajectory_planner_B.bid1;
5117 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 2] = 0.0;
5118 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
5119 5] = 0.0;
5120 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 5] =
5121 cartesian_trajectory_planner_B.bid1;
5122 }
5123
5124 cartesian_trajectory_planner_B.n_h = Jac->size[1];
5125 cartesian_trajectory_planner_B.b_kstr = y->size[0] * y->size[1];
5126 y->size[0] = 6;
5127 y->size[1] = Jac->size[1];
5128 cartes_emxEnsureCapacity_real_T(y, cartesian_trajectory_planner_B.b_kstr);
5129 cartesian_trajectory_planner_B.loop_ub_i = Jac->size[0] * Jac->size[1] - 1;
5130 for (cartesian_trajectory_planner_B.b_kstr = 0;
5131 cartesian_trajectory_planner_B.b_kstr <=
5132 cartesian_trajectory_planner_B.loop_ub_i;
5133 cartesian_trajectory_planner_B.b_kstr++) {
5134 y->data[cartesian_trajectory_planner_B.b_kstr] = Jac->
5135 data[cartesian_trajectory_planner_B.b_kstr];
5136 }
5137
5138 cartesian_trajectory_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
5139 Jac->size[0] = 6;
5140 Jac->size[1] = cartesian_trajectory_planner_B.n_h;
5141 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr);
5142 for (cartesian_trajectory_planner_B.b_kstr = 0;
5143 cartesian_trajectory_planner_B.b_kstr <
5144 cartesian_trajectory_planner_B.n_h; cartesian_trajectory_planner_B.b_kstr
5145 ++) {
5146 cartesian_trajectory_planner_B.coffset_tmp =
5147 cartesian_trajectory_planner_B.b_kstr * 6 - 1;
5148 for (cartesian_trajectory_planner_B.b_i_c = 0;
5149 cartesian_trajectory_planner_B.b_i_c < 6;
5150 cartesian_trajectory_planner_B.b_i_c++) {
5151 cartesian_trajectory_planner_B.bid2 = 0.0;
5152 for (cartesian_trajectory_planner_B.loop_ub_i = 0;
5153 cartesian_trajectory_planner_B.loop_ub_i < 6;
5154 cartesian_trajectory_planner_B.loop_ub_i++) {
5155 cartesian_trajectory_planner_B.bid2 +=
5156 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_i
5157 * 6 + cartesian_trajectory_planner_B.b_i_c] * y->data
5158 [(cartesian_trajectory_planner_B.coffset_tmp +
5159 cartesian_trajectory_planner_B.loop_ub_i) + 1];
5160 }
5161
5162 Jac->data[(cartesian_trajectory_planner_B.coffset_tmp +
5163 cartesian_trajectory_planner_B.b_i_c) + 1] =
5164 cartesian_trajectory_planner_B.bid2;
5165 }
5166 }
5167
5168 cartesian_trajec_emxFree_real_T(&y);
5169 T_size[0] = 4;
5170 T_size[1] = 4;
5171 memcpy(&T_data[0], &cartesian_trajectory_planner_B.T1[0], sizeof(real_T) << 4U);
5172}
5173
5174real_T rt_hypotd_snf(real_T u0, real_T u1)
5175{
5176 real_T y;
5177 real_T a;
5178 a = fabs(u0);
5179 y = fabs(u1);
5180 if (a < y) {
5181 a /= y;
5182 y *= sqrt(a * a + 1.0);
5183 } else if (a > y) {
5184 y /= a;
5185 y = sqrt(y * y + 1.0) * a;
5186 } else {
5187 if (!rtIsNaN(y)) {
5188 y = a * 1.4142135623730951;
5189 }
5190 }
5191
5192 return y;
5193}
5194
5195static creal_T cartesian_trajectory_plann_sqrt(const creal_T x)
5196{
5197 creal_T b_x;
5198 real_T absxr;
5199 real_T absxi;
5200 if (x.im == 0.0) {
5201 if (x.re < 0.0) {
5202 absxr = 0.0;
5203 absxi = sqrt(-x.re);
5204 } else {
5205 absxr = sqrt(x.re);
5206 absxi = 0.0;
5207 }
5208 } else if (x.re == 0.0) {
5209 if (x.im < 0.0) {
5210 absxr = sqrt(-x.im / 2.0);
5211 absxi = -absxr;
5212 } else {
5213 absxr = sqrt(x.im / 2.0);
5214 absxi = absxr;
5215 }
5216 } else if (rtIsNaN(x.re)) {
5217 absxr = x.re;
5218 absxi = x.re;
5219 } else if (rtIsNaN(x.im)) {
5220 absxr = x.im;
5221 absxi = x.im;
5222 } else if (rtIsInf(x.im)) {
5223 absxr = fabs(x.im);
5224 absxi = x.im;
5225 } else if (rtIsInf(x.re)) {
5226 if (x.re < 0.0) {
5227 absxr = 0.0;
5228 absxi = x.im * -x.re;
5229 } else {
5230 absxr = x.re;
5231 absxi = 0.0;
5232 }
5233 } else {
5234 absxr = fabs(x.re);
5235 absxi = fabs(x.im);
5236 if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307))
5237 {
5238 absxr *= 0.5;
5239 absxi *= 0.5;
5240 absxi = rt_hypotd_snf(absxr, absxi);
5241 if (absxi > absxr) {
5242 absxr = sqrt(absxr / absxi + 1.0) * sqrt(absxi);
5243 } else {
5244 absxr = sqrt(absxi) * 1.4142135623730951;
5245 }
5246 } else {
5247 absxr = sqrt((rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);
5248 }
5249
5250 if (x.re > 0.0) {
5251 absxi = x.im / absxr * 0.5;
5252 } else {
5253 if (x.im < 0.0) {
5254 absxi = -absxr;
5255 } else {
5256 absxi = absxr;
5257 }
5258
5259 absxr = x.im / absxi * 0.5;
5260 }
5261 }
5262
5263 b_x.re = absxr;
5264 b_x.im = absxi;
5265 return b_x;
5266}
5267
5268real_T rt_atan2d_snf(real_T u0, real_T u1)
5269{
5270 real_T y;
5271 int32_T u0_0;
5272 int32_T u1_0;
5273 if (rtIsNaN(u0) || rtIsNaN(u1)) {
5274 y = (rtNaN);
5275 } else if (rtIsInf(u0) && rtIsInf(u1)) {
5276 if (u0 > 0.0) {
5277 u0_0 = 1;
5278 } else {
5279 u0_0 = -1;
5280 }
5281
5282 if (u1 > 0.0) {
5283 u1_0 = 1;
5284 } else {
5285 u1_0 = -1;
5286 }
5287
5288 y = atan2(static_cast<real_T>(u0_0), static_cast<real_T>(u1_0));
5289 } else if (u1 == 0.0) {
5290 if (u0 > 0.0) {
5291 y = RT_PI / 2.0;
5292 } else if (u0 < 0.0) {
5293 y = -(RT_PI / 2.0);
5294 } else {
5295 y = 0.0;
5296 }
5297 } else {
5298 y = atan2(u0, u1);
5299 }
5300
5301 return y;
5302}
5303
5304static real_T cartesian_trajectory_plan_xnrm2(int32_T n, const real_T x[9],
5305 int32_T ix0)
5306{
5307 real_T y;
5308 real_T scale;
5309 int32_T kend;
5310 real_T absxk;
5311 real_T t;
5312 int32_T k;
5313 y = 0.0;
5314 scale = 3.3121686421112381E-170;
5315 kend = ix0 + n;
5316 for (k = ix0; k < kend; k++) {
5317 absxk = fabs(x[k - 1]);
5318 if (absxk > scale) {
5319 t = scale / absxk;
5320 y = y * t * t + 1.0;
5321 scale = absxk;
5322 } else {
5323 t = absxk / scale;
5324 y += t * t;
5325 }
5326 }
5327
5328 return scale * sqrt(y);
5329}
5330
5331static real_T cartesian_trajectory_plan_xdotc(int32_T n, const real_T x[9],
5332 int32_T ix0, const real_T y[9], int32_T iy0)
5333{
5334 real_T d;
5335 int32_T ix;
5336 int32_T iy;
5337 int32_T k;
5338 d = 0.0;
5339 ix = ix0 - 1;
5340 iy = iy0 - 1;
5341 for (k = 0; k < n; k++) {
5342 d += x[ix] * y[iy];
5343 ix++;
5344 iy++;
5345 }
5346
5347 return d;
5348}
5349
5350static void cartesian_trajectory_plan_xaxpy(int32_T n, real_T a, int32_T ix0,
5351 const real_T y[9], int32_T iy0, real_T b_y[9])
5352{
5353 int32_T ix;
5354 int32_T iy;
5355 int32_T k;
5356 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
5357 if (!(a == 0.0)) {
5358 ix = ix0;
5359 iy = iy0 - 1;
5360 for (k = 0; k < n; k++) {
5361 b_y[iy] += b_y[ix - 1] * a;
5362 ix++;
5363 iy++;
5364 }
5365 }
5366}
5367
5368static real_T cartesian_trajectory_pl_xnrm2_a(const real_T x[3], int32_T ix0)
5369{
5370 real_T y;
5371 real_T scale;
5372 real_T absxk;
5373 real_T t;
5374 int32_T k;
5375 y = 0.0;
5376 scale = 3.3121686421112381E-170;
5377 for (k = ix0; k <= ix0 + 1; k++) {
5378 absxk = fabs(x[k - 1]);
5379 if (absxk > scale) {
5380 t = scale / absxk;
5381 y = y * t * t + 1.0;
5382 scale = absxk;
5383 } else {
5384 t = absxk / scale;
5385 y += t * t;
5386 }
5387 }
5388
5389 return scale * sqrt(y);
5390}
5391
5392static void cartesian_trajectory__xaxpy_ast(int32_T n, real_T a, const real_T x
5393 [9], int32_T ix0, real_T y[3], int32_T iy0)
5394{
5395 int32_T ix;
5396 int32_T iy;
5397 int32_T k;
5398 if (!(a == 0.0)) {
5399 ix = ix0;
5400 iy = iy0 - 1;
5401 for (k = 0; k < n; k++) {
5402 y[iy] += x[ix - 1] * a;
5403 ix++;
5404 iy++;
5405 }
5406 }
5407}
5408
5409static void cartesian_trajectory_p_xaxpy_as(int32_T n, real_T a, const real_T x
5410 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9])
5411{
5412 int32_T ix;
5413 int32_T iy;
5414 int32_T k;
5415 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
5416 if (!(a == 0.0)) {
5417 ix = ix0;
5418 iy = iy0 - 1;
5419 for (k = 0; k < n; k++) {
5420 b_y[iy] += x[ix - 1] * a;
5421 ix++;
5422 iy++;
5423 }
5424 }
5425}
5426
5427static void cartesian_trajectory_plan_xswap(const real_T x[9], int32_T ix0,
5428 int32_T iy0, real_T b_x[9])
5429{
5430 int32_T ix;
5431 int32_T iy;
5432 real_T temp;
5433 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
5434 ix = ix0 - 1;
5435 iy = iy0 - 1;
5436 temp = b_x[ix];
5437 b_x[ix] = b_x[iy];
5438 b_x[iy] = temp;
5439 ix++;
5440 iy++;
5441 temp = b_x[ix];
5442 b_x[ix] = b_x[iy];
5443 b_x[iy] = temp;
5444 ix++;
5445 iy++;
5446 temp = b_x[ix];
5447 b_x[ix] = b_x[iy];
5448 b_x[iy] = temp;
5449}
5450
5451static void cartesian_trajectory_plan_xrotg(real_T a, real_T b, real_T *b_a,
5452 real_T *b_b, real_T *c, real_T *s)
5453{
5454 cartesian_trajectory_planner_B.roe = b;
5455 cartesian_trajectory_planner_B.absa = fabs(a);
5456 cartesian_trajectory_planner_B.absb = fabs(b);
5457 if (cartesian_trajectory_planner_B.absa > cartesian_trajectory_planner_B.absb)
5458 {
5459 cartesian_trajectory_planner_B.roe = a;
5460 }
5461
5462 cartesian_trajectory_planner_B.scale = cartesian_trajectory_planner_B.absa +
5463 cartesian_trajectory_planner_B.absb;
5464 if (cartesian_trajectory_planner_B.scale == 0.0) {
5465 *s = 0.0;
5466 *c = 1.0;
5467 *b_a = 0.0;
5468 *b_b = 0.0;
5469 } else {
5470 cartesian_trajectory_planner_B.ads = cartesian_trajectory_planner_B.absa /
5471 cartesian_trajectory_planner_B.scale;
5472 cartesian_trajectory_planner_B.bds = cartesian_trajectory_planner_B.absb /
5473 cartesian_trajectory_planner_B.scale;
5474 *b_a = sqrt(cartesian_trajectory_planner_B.ads *
5475 cartesian_trajectory_planner_B.ads +
5476 cartesian_trajectory_planner_B.bds *
5477 cartesian_trajectory_planner_B.bds) *
5478 cartesian_trajectory_planner_B.scale;
5479 if (cartesian_trajectory_planner_B.roe < 0.0) {
5480 *b_a = -*b_a;
5481 }
5482
5483 *c = a / *b_a;
5484 *s = b / *b_a;
5485 if (cartesian_trajectory_planner_B.absa >
5486 cartesian_trajectory_planner_B.absb) {
5487 *b_b = *s;
5488 } else if (*c != 0.0) {
5489 *b_b = 1.0 / *c;
5490 } else {
5491 *b_b = 1.0;
5492 }
5493 }
5494}
5495
5496static void cartesian_trajectory_plann_xrot(const real_T x[9], int32_T ix0,
5497 int32_T iy0, real_T c, real_T s, real_T b_x[9])
5498{
5499 int32_T ix;
5500 int32_T iy;
5501 real_T temp;
5502 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
5503 ix = ix0 - 1;
5504 iy = iy0 - 1;
5505 temp = c * b_x[ix] + s * b_x[iy];
5506 b_x[iy] = c * b_x[iy] - s * b_x[ix];
5507 b_x[ix] = temp;
5508 iy++;
5509 ix++;
5510 temp = c * b_x[ix] + s * b_x[iy];
5511 b_x[iy] = c * b_x[iy] - s * b_x[ix];
5512 b_x[ix] = temp;
5513 iy++;
5514 ix++;
5515 temp = c * b_x[ix] + s * b_x[iy];
5516 b_x[iy] = c * b_x[iy] - s * b_x[ix];
5517 b_x[ix] = temp;
5518}
5519
5520static void cartesian_trajectory_planne_svd(const real_T A[9], real_T U[9],
5521 real_T s[3], real_T V[9])
5522{
5523 int32_T qq;
5524 boolean_T apply_transform;
5525 int32_T qjj;
5526 int32_T m;
5527 int32_T kase;
5528 int32_T c_q;
5529 int32_T d_k;
5530 boolean_T exitg1;
5531 cartesian_trajectory_planner_B.e_e[0] = 0.0;
5532 cartesian_trajectory_planner_B.work[0] = 0.0;
5533 cartesian_trajectory_planner_B.e_e[1] = 0.0;
5534 cartesian_trajectory_planner_B.work[1] = 0.0;
5535 cartesian_trajectory_planner_B.e_e[2] = 0.0;
5536 cartesian_trajectory_planner_B.work[2] = 0.0;
5537 for (m = 0; m < 9; m++) {
5538 cartesian_trajectory_planner_B.A[m] = A[m];
5539 U[m] = 0.0;
5540 V[m] = 0.0;
5541 }
5542
5543 apply_transform = false;
5544 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_plan_xnrm2(3,
5545 cartesian_trajectory_planner_B.A, 1);
5546 if (cartesian_trajectory_planner_B.nrm > 0.0) {
5547 apply_transform = true;
5548 if (cartesian_trajectory_planner_B.A[0] < 0.0) {
5549 cartesian_trajectory_planner_B.s[0] = -cartesian_trajectory_planner_B.nrm;
5550 } else {
5551 cartesian_trajectory_planner_B.s[0] = cartesian_trajectory_planner_B.nrm;
5552 }
5553
5554 if (fabs(cartesian_trajectory_planner_B.s[0]) >= 1.0020841800044864E-292) {
5555 cartesian_trajectory_planner_B.nrm = 1.0 /
5556 cartesian_trajectory_planner_B.s[0];
5557 for (qq = 1; qq < 4; qq++) {
5558 cartesian_trajectory_planner_B.A[qq - 1] *=
5559 cartesian_trajectory_planner_B.nrm;
5560 }
5561 } else {
5562 for (qq = 1; qq < 4; qq++) {
5563 cartesian_trajectory_planner_B.A[qq - 1] /=
5564 cartesian_trajectory_planner_B.s[0];
5565 }
5566 }
5567
5568 cartesian_trajectory_planner_B.A[0]++;
5569 cartesian_trajectory_planner_B.s[0] = -cartesian_trajectory_planner_B.s[0];
5570 } else {
5571 cartesian_trajectory_planner_B.s[0] = 0.0;
5572 }
5573
5574 for (m = 2; m < 4; m++) {
5575 qjj = (m - 1) * 3 + 1;
5576 if (apply_transform) {
5577 memcpy(&cartesian_trajectory_planner_B.A_p[0],
5578 &cartesian_trajectory_planner_B.A[0], 9U * sizeof(real_T));
5579 cartesian_trajectory_plan_xaxpy(3, -(cartesian_trajectory_plan_xdotc(3,
5580 cartesian_trajectory_planner_B.A, 1, cartesian_trajectory_planner_B.A,
5581 qjj) / cartesian_trajectory_planner_B.A[0]), 1,
5582 cartesian_trajectory_planner_B.A_p, qjj,
5583 cartesian_trajectory_planner_B.A);
5584 }
5585
5586 cartesian_trajectory_planner_B.e_e[m - 1] =
5587 cartesian_trajectory_planner_B.A[qjj - 1];
5588 }
5589
5590 for (m = 1; m < 4; m++) {
5591 U[m - 1] = cartesian_trajectory_planner_B.A[m - 1];
5592 }
5593
5594 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_pl_xnrm2_a
5595 (cartesian_trajectory_planner_B.e_e, 2);
5596 if (cartesian_trajectory_planner_B.nrm == 0.0) {
5597 cartesian_trajectory_planner_B.e_e[0] = 0.0;
5598 } else {
5599 if (cartesian_trajectory_planner_B.e_e[1] < 0.0) {
5600 cartesian_trajectory_planner_B.rt = -cartesian_trajectory_planner_B.nrm;
5601 cartesian_trajectory_planner_B.e_e[0] =
5602 -cartesian_trajectory_planner_B.nrm;
5603 } else {
5604 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.nrm;
5605 cartesian_trajectory_planner_B.e_e[0] = cartesian_trajectory_planner_B.nrm;
5606 }
5607
5608 if (fabs(cartesian_trajectory_planner_B.rt) >= 1.0020841800044864E-292) {
5609 cartesian_trajectory_planner_B.nrm = 1.0 /
5610 cartesian_trajectory_planner_B.rt;
5611 for (qq = 2; qq < 4; qq++) {
5612 cartesian_trajectory_planner_B.e_e[qq - 1] *=
5613 cartesian_trajectory_planner_B.nrm;
5614 }
5615 } else {
5616 for (qq = 2; qq < 4; qq++) {
5617 cartesian_trajectory_planner_B.e_e[qq - 1] /=
5618 cartesian_trajectory_planner_B.rt;
5619 }
5620 }
5621
5622 cartesian_trajectory_planner_B.e_e[1]++;
5623 cartesian_trajectory_planner_B.e_e[0] = -cartesian_trajectory_planner_B.e_e
5624 [0];
5625 for (m = 2; m < 4; m++) {
5626 cartesian_trajectory_planner_B.work[m - 1] = 0.0;
5627 }
5628
5629 for (m = 2; m < 4; m++) {
5630 cartesian_trajectory__xaxpy_ast(2, cartesian_trajectory_planner_B.e_e[m -
5631 1], cartesian_trajectory_planner_B.A, 3 * (m - 1) + 2,
5632 cartesian_trajectory_planner_B.work, 2);
5633 }
5634
5635 for (m = 2; m < 4; m++) {
5636 memcpy(&cartesian_trajectory_planner_B.A_p[0],
5637 &cartesian_trajectory_planner_B.A[0], 9U * sizeof(real_T));
5638 cartesian_trajectory_p_xaxpy_as(2, -cartesian_trajectory_planner_B.e_e[m -
5639 1] / cartesian_trajectory_planner_B.e_e[1],
5640 cartesian_trajectory_planner_B.work, 2,
5641 cartesian_trajectory_planner_B.A_p, (m - 1) * 3 + 2,
5642 cartesian_trajectory_planner_B.A);
5643 }
5644 }
5645
5646 for (m = 2; m < 4; m++) {
5647 V[m - 1] = cartesian_trajectory_planner_B.e_e[m - 1];
5648 }
5649
5650 apply_transform = false;
5651 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_plan_xnrm2(2,
5652 cartesian_trajectory_planner_B.A, 5);
5653 if (cartesian_trajectory_planner_B.nrm > 0.0) {
5654 apply_transform = true;
5655 if (cartesian_trajectory_planner_B.A[4] < 0.0) {
5656 cartesian_trajectory_planner_B.s[1] = -cartesian_trajectory_planner_B.nrm;
5657 } else {
5658 cartesian_trajectory_planner_B.s[1] = cartesian_trajectory_planner_B.nrm;
5659 }
5660
5661 if (fabs(cartesian_trajectory_planner_B.s[1]) >= 1.0020841800044864E-292) {
5662 cartesian_trajectory_planner_B.nrm = 1.0 /
5663 cartesian_trajectory_planner_B.s[1];
5664 for (qq = 5; qq < 7; qq++) {
5665 cartesian_trajectory_planner_B.A[qq - 1] *=
5666 cartesian_trajectory_planner_B.nrm;
5667 }
5668 } else {
5669 for (qq = 5; qq < 7; qq++) {
5670 cartesian_trajectory_planner_B.A[qq - 1] /=
5671 cartesian_trajectory_planner_B.s[1];
5672 }
5673 }
5674
5675 cartesian_trajectory_planner_B.A[4]++;
5676 cartesian_trajectory_planner_B.s[1] = -cartesian_trajectory_planner_B.s[1];
5677 } else {
5678 cartesian_trajectory_planner_B.s[1] = 0.0;
5679 }
5680
5681 if (apply_transform) {
5682 for (m = 3; m < 4; m++) {
5683 memcpy(&cartesian_trajectory_planner_B.A_p[0],
5684 &cartesian_trajectory_planner_B.A[0], 9U * sizeof(real_T));
5685 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2,
5686 cartesian_trajectory_planner_B.A, 5, cartesian_trajectory_planner_B.A, 8)
5687 / cartesian_trajectory_planner_B.A[4]), 5,
5688 cartesian_trajectory_planner_B.A_p, 8, cartesian_trajectory_planner_B.A);
5689 }
5690 }
5691
5692 for (m = 2; m < 4; m++) {
5693 U[m + 2] = cartesian_trajectory_planner_B.A[m + 2];
5694 }
5695
5696 m = 2;
5697 cartesian_trajectory_planner_B.s[2] = cartesian_trajectory_planner_B.A[8];
5698 cartesian_trajectory_planner_B.e_e[1] = cartesian_trajectory_planner_B.A[7];
5699 cartesian_trajectory_planner_B.e_e[2] = 0.0;
5700 U[6] = 0.0;
5701 U[7] = 0.0;
5702 U[8] = 1.0;
5703 for (c_q = 1; c_q >= 0; c_q--) {
5704 qq = 3 * c_q + c_q;
5705 if (cartesian_trajectory_planner_B.s[c_q] != 0.0) {
5706 for (kase = c_q + 2; kase < 4; kase++) {
5707 qjj = ((kase - 1) * 3 + c_q) + 1;
5708 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
5709 cartesian_trajectory_plan_xaxpy(3 - c_q,
5710 -(cartesian_trajectory_plan_xdotc(3 - c_q, U, qq + 1, U, qjj) / U[qq]),
5711 qq + 1, cartesian_trajectory_planner_B.A, qjj, U);
5712 }
5713
5714 for (qjj = c_q + 1; qjj < 4; qjj++) {
5715 kase = (3 * c_q + qjj) - 1;
5716 U[kase] = -U[kase];
5717 }
5718
5719 U[qq]++;
5720 if (0 <= c_q - 1) {
5721 U[3 * c_q] = 0.0;
5722 }
5723 } else {
5724 U[3 * c_q] = 0.0;
5725 U[3 * c_q + 1] = 0.0;
5726 U[3 * c_q + 2] = 0.0;
5727 U[qq] = 1.0;
5728 }
5729 }
5730
5731 for (c_q = 2; c_q >= 0; c_q--) {
5732 if ((c_q + 1 <= 1) && (cartesian_trajectory_planner_B.e_e[0] != 0.0)) {
5733 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
5734 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2, V,
5735 2, V, 5) / V[1]), 2, cartesian_trajectory_planner_B.A, 5, V);
5736 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
5737 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2, V,
5738 2, V, 8) / V[1]), 2, cartesian_trajectory_planner_B.A, 8, V);
5739 }
5740
5741 V[3 * c_q] = 0.0;
5742 V[3 * c_q + 1] = 0.0;
5743 V[3 * c_q + 2] = 0.0;
5744 V[c_q + 3 * c_q] = 1.0;
5745 }
5746
5747 for (c_q = 0; c_q < 3; c_q++) {
5748 cartesian_trajectory_planner_B.ztest =
5749 cartesian_trajectory_planner_B.e_e[c_q];
5750 if (cartesian_trajectory_planner_B.s[c_q] != 0.0) {
5751 cartesian_trajectory_planner_B.rt = fabs
5752 (cartesian_trajectory_planner_B.s[c_q]);
5753 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.s[c_q]
5754 / cartesian_trajectory_planner_B.rt;
5755 cartesian_trajectory_planner_B.s[c_q] = cartesian_trajectory_planner_B.rt;
5756 if (c_q + 1 < 3) {
5757 cartesian_trajectory_planner_B.ztest =
5758 cartesian_trajectory_planner_B.e_e[c_q] /
5759 cartesian_trajectory_planner_B.nrm;
5760 }
5761
5762 qjj = 3 * c_q;
5763 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
5764 U[qq - 1] *= cartesian_trajectory_planner_B.nrm;
5765 }
5766 }
5767
5768 if ((c_q + 1 < 3) && (cartesian_trajectory_planner_B.ztest != 0.0)) {
5769 cartesian_trajectory_planner_B.rt = fabs
5770 (cartesian_trajectory_planner_B.ztest);
5771 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt /
5772 cartesian_trajectory_planner_B.ztest;
5773 cartesian_trajectory_planner_B.ztest = cartesian_trajectory_planner_B.rt;
5774 cartesian_trajectory_planner_B.s[c_q + 1] *=
5775 cartesian_trajectory_planner_B.nrm;
5776 qjj = (c_q + 1) * 3;
5777 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
5778 V[qq - 1] *= cartesian_trajectory_planner_B.nrm;
5779 }
5780 }
5781
5782 cartesian_trajectory_planner_B.e_e[c_q] =
5783 cartesian_trajectory_planner_B.ztest;
5784 }
5785
5786 qq = 0;
5787 cartesian_trajectory_planner_B.nrm = 0.0;
5788 cartesian_trajectory_planner_B.ztest = fabs(cartesian_trajectory_planner_B.s[0]);
5789 cartesian_trajectory_planner_B.rt = fabs(cartesian_trajectory_planner_B.e_e[0]);
5790 if ((cartesian_trajectory_planner_B.ztest > cartesian_trajectory_planner_B.rt)
5791 || rtIsNaN(cartesian_trajectory_planner_B.rt)) {
5792 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
5793 }
5794
5795 if (!rtIsNaN(cartesian_trajectory_planner_B.rt)) {
5796 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt;
5797 }
5798
5799 cartesian_trajectory_planner_B.ztest = fabs(cartesian_trajectory_planner_B.s[1]);
5800 cartesian_trajectory_planner_B.rt = fabs(cartesian_trajectory_planner_B.e_e[1]);
5801 if ((cartesian_trajectory_planner_B.ztest > cartesian_trajectory_planner_B.rt)
5802 || rtIsNaN(cartesian_trajectory_planner_B.rt)) {
5803 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
5804 }
5805
5806 if ((!(cartesian_trajectory_planner_B.nrm > cartesian_trajectory_planner_B.rt))
5807 && (!rtIsNaN(cartesian_trajectory_planner_B.rt))) {
5808 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt;
5809 }
5810
5811 cartesian_trajectory_planner_B.ztest = fabs(cartesian_trajectory_planner_B.s[2]);
5812 cartesian_trajectory_planner_B.rt = fabs(cartesian_trajectory_planner_B.e_e[2]);
5813 if ((cartesian_trajectory_planner_B.ztest > cartesian_trajectory_planner_B.rt)
5814 || rtIsNaN(cartesian_trajectory_planner_B.rt)) {
5815 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
5816 }
5817
5818 if ((!(cartesian_trajectory_planner_B.nrm > cartesian_trajectory_planner_B.rt))
5819 && (!rtIsNaN(cartesian_trajectory_planner_B.rt))) {
5820 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt;
5821 }
5822
5823 while ((m + 1 > 0) && (!(qq >= 75))) {
5824 c_q = m;
5825 qjj = m;
5826 exitg1 = false;
5827 while ((!exitg1) && (qjj > -1)) {
5828 c_q = qjj;
5829 if (qjj == 0) {
5830 exitg1 = true;
5831 } else {
5832 cartesian_trajectory_planner_B.rt = fabs
5833 (cartesian_trajectory_planner_B.e_e[qjj - 1]);
5834 if ((cartesian_trajectory_planner_B.rt <= (fabs
5835 (cartesian_trajectory_planner_B.s[qjj - 1]) + fabs
5836 (cartesian_trajectory_planner_B.s[qjj])) * 2.2204460492503131E-16)
5837 || (cartesian_trajectory_planner_B.rt <= 1.0020841800044864E-292) ||
5838 ((qq > 20) && (cartesian_trajectory_planner_B.rt <=
5839 2.2204460492503131E-16 *
5840 cartesian_trajectory_planner_B.nrm))) {
5841 cartesian_trajectory_planner_B.e_e[qjj - 1] = 0.0;
5842 exitg1 = true;
5843 } else {
5844 qjj--;
5845 }
5846 }
5847 }
5848
5849 if (c_q == m) {
5850 kase = 4;
5851 } else {
5852 qjj = m + 1;
5853 kase = m + 1;
5854 exitg1 = false;
5855 while ((!exitg1) && (kase >= c_q)) {
5856 qjj = kase;
5857 if (kase == c_q) {
5858 exitg1 = true;
5859 } else {
5860 cartesian_trajectory_planner_B.rt = 0.0;
5861 if (kase < m + 1) {
5862 cartesian_trajectory_planner_B.rt = fabs
5863 (cartesian_trajectory_planner_B.e_e[kase - 1]);
5864 }
5865
5866 if (kase > c_q + 1) {
5867 cartesian_trajectory_planner_B.rt += fabs
5868 (cartesian_trajectory_planner_B.e_e[kase - 2]);
5869 }
5870
5871 cartesian_trajectory_planner_B.ztest = fabs
5872 (cartesian_trajectory_planner_B.s[kase - 1]);
5873 if ((cartesian_trajectory_planner_B.ztest <= 2.2204460492503131E-16 *
5874 cartesian_trajectory_planner_B.rt) ||
5875 (cartesian_trajectory_planner_B.ztest <= 1.0020841800044864E-292))
5876 {
5877 cartesian_trajectory_planner_B.s[kase - 1] = 0.0;
5878 exitg1 = true;
5879 } else {
5880 kase--;
5881 }
5882 }
5883 }
5884
5885 if (qjj == c_q) {
5886 kase = 3;
5887 } else if (m + 1 == qjj) {
5888 kase = 1;
5889 } else {
5890 kase = 2;
5891 c_q = qjj;
5892 }
5893 }
5894
5895 switch (kase) {
5896 case 1:
5897 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.e_e[m -
5898 1];
5899 cartesian_trajectory_planner_B.e_e[m - 1] = 0.0;
5900 for (qjj = m; qjj >= c_q + 1; qjj--) {
5901 cartesian_trajectory_planner_B.ztest =
5902 cartesian_trajectory_planner_B.e_e[0];
5903 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.s[qjj - 1],
5904 cartesian_trajectory_planner_B.rt,
5905 &cartesian_trajectory_planner_B.s[qjj - 1],
5906 &cartesian_trajectory_planner_B.rt,
5907 &cartesian_trajectory_planner_B.sqds,
5908 &cartesian_trajectory_planner_B.b_c5);
5909 if (qjj > c_q + 1) {
5910 cartesian_trajectory_planner_B.rt =
5911 -cartesian_trajectory_planner_B.b_c5 *
5912 cartesian_trajectory_planner_B.e_e[0];
5913 cartesian_trajectory_planner_B.ztest =
5914 cartesian_trajectory_planner_B.e_e[0] *
5915 cartesian_trajectory_planner_B.sqds;
5916 }
5917
5918 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
5919 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, (qjj -
5920 1) * 3 + 1, 3 * m + 1, cartesian_trajectory_planner_B.sqds,
5921 cartesian_trajectory_planner_B.b_c5, V);
5922 cartesian_trajectory_planner_B.e_e[0] =
5923 cartesian_trajectory_planner_B.ztest;
5924 }
5925 break;
5926
5927 case 2:
5928 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.e_e[c_q
5929 - 1];
5930 cartesian_trajectory_planner_B.e_e[c_q - 1] = 0.0;
5931 for (qjj = c_q + 1; qjj <= m + 1; qjj++) {
5932 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.s[qjj - 1],
5933 cartesian_trajectory_planner_B.rt,
5934 &cartesian_trajectory_planner_B.s[qjj - 1],
5935 &cartesian_trajectory_planner_B.ztest,
5936 &cartesian_trajectory_planner_B.sqds,
5937 &cartesian_trajectory_planner_B.b_c5);
5938 cartesian_trajectory_planner_B.ztest =
5939 cartesian_trajectory_planner_B.e_e[qjj - 1];
5940 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest
5941 * -cartesian_trajectory_planner_B.b_c5;
5942 cartesian_trajectory_planner_B.e_e[qjj - 1] =
5943 cartesian_trajectory_planner_B.ztest *
5944 cartesian_trajectory_planner_B.sqds;
5945 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
5946 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, (qjj -
5947 1) * 3 + 1, (c_q - 1) * 3 + 1, cartesian_trajectory_planner_B.sqds,
5948 cartesian_trajectory_planner_B.b_c5, U);
5949 }
5950 break;
5951
5952 case 3:
5953 cartesian_trajectory_planner_B.ztest = fabs
5954 (cartesian_trajectory_planner_B.s[m]);
5955 cartesian_trajectory_planner_B.sqds = cartesian_trajectory_planner_B.s[m -
5956 1];
5957 cartesian_trajectory_planner_B.rt = fabs
5958 (cartesian_trajectory_planner_B.sqds);
5959 if ((cartesian_trajectory_planner_B.ztest >
5960 cartesian_trajectory_planner_B.rt) || rtIsNaN
5961 (cartesian_trajectory_planner_B.rt)) {
5962 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
5963 }
5964
5965 cartesian_trajectory_planner_B.b_c5 = cartesian_trajectory_planner_B.e_e[m
5966 - 1];
5967 cartesian_trajectory_planner_B.ztest = fabs
5968 (cartesian_trajectory_planner_B.b_c5);
5969 if ((cartesian_trajectory_planner_B.rt >
5970 cartesian_trajectory_planner_B.ztest) || rtIsNaN
5971 (cartesian_trajectory_planner_B.ztest)) {
5972 cartesian_trajectory_planner_B.ztest = cartesian_trajectory_planner_B.rt;
5973 }
5974
5975 cartesian_trajectory_planner_B.rt = fabs
5976 (cartesian_trajectory_planner_B.s[c_q]);
5977 if ((cartesian_trajectory_planner_B.ztest >
5978 cartesian_trajectory_planner_B.rt) || rtIsNaN
5979 (cartesian_trajectory_planner_B.rt)) {
5980 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
5981 }
5982
5983 cartesian_trajectory_planner_B.ztest = fabs
5984 (cartesian_trajectory_planner_B.e_e[c_q]);
5985 if ((cartesian_trajectory_planner_B.rt >
5986 cartesian_trajectory_planner_B.ztest) || rtIsNaN
5987 (cartesian_trajectory_planner_B.ztest)) {
5988 cartesian_trajectory_planner_B.ztest = cartesian_trajectory_planner_B.rt;
5989 }
5990
5991 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.s[m] /
5992 cartesian_trajectory_planner_B.ztest;
5993 cartesian_trajectory_planner_B.smm1 = cartesian_trajectory_planner_B.sqds /
5994 cartesian_trajectory_planner_B.ztest;
5995 cartesian_trajectory_planner_B.emm1 = cartesian_trajectory_planner_B.b_c5 /
5996 cartesian_trajectory_planner_B.ztest;
5997 cartesian_trajectory_planner_B.sqds = cartesian_trajectory_planner_B.s[c_q]
5998 / cartesian_trajectory_planner_B.ztest;
5999 cartesian_trajectory_planner_B.b_c5 =
6000 ((cartesian_trajectory_planner_B.smm1 +
6001 cartesian_trajectory_planner_B.rt) *
6002 (cartesian_trajectory_planner_B.smm1 -
6003 cartesian_trajectory_planner_B.rt) +
6004 cartesian_trajectory_planner_B.emm1 *
6005 cartesian_trajectory_planner_B.emm1) / 2.0;
6006 cartesian_trajectory_planner_B.smm1 = cartesian_trajectory_planner_B.rt *
6007 cartesian_trajectory_planner_B.emm1;
6008 cartesian_trajectory_planner_B.smm1 *= cartesian_trajectory_planner_B.smm1;
6009 if ((cartesian_trajectory_planner_B.b_c5 != 0.0) ||
6010 (cartesian_trajectory_planner_B.smm1 != 0.0)) {
6011 cartesian_trajectory_planner_B.emm1 = sqrt
6012 (cartesian_trajectory_planner_B.b_c5 *
6013 cartesian_trajectory_planner_B.b_c5 +
6014 cartesian_trajectory_planner_B.smm1);
6015 if (cartesian_trajectory_planner_B.b_c5 < 0.0) {
6016 cartesian_trajectory_planner_B.emm1 =
6017 -cartesian_trajectory_planner_B.emm1;
6018 }
6019
6020 cartesian_trajectory_planner_B.emm1 =
6021 cartesian_trajectory_planner_B.smm1 /
6022 (cartesian_trajectory_planner_B.b_c5 +
6023 cartesian_trajectory_planner_B.emm1);
6024 } else {
6025 cartesian_trajectory_planner_B.emm1 = 0.0;
6026 }
6027
6028 cartesian_trajectory_planner_B.rt = (cartesian_trajectory_planner_B.sqds +
6029 cartesian_trajectory_planner_B.rt) *
6030 (cartesian_trajectory_planner_B.sqds - cartesian_trajectory_planner_B.rt)
6031 + cartesian_trajectory_planner_B.emm1;
6032 cartesian_trajectory_planner_B.sqds *=
6033 cartesian_trajectory_planner_B.e_e[c_q] /
6034 cartesian_trajectory_planner_B.ztest;
6035 for (d_k = c_q + 1; d_k <= m; d_k++) {
6036 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.rt,
6037 cartesian_trajectory_planner_B.sqds,
6038 &cartesian_trajectory_planner_B.ztest,
6039 &cartesian_trajectory_planner_B.emm1,
6040 &cartesian_trajectory_planner_B.b_c5,
6041 &cartesian_trajectory_planner_B.smm1);
6042 if (d_k > c_q + 1) {
6043 cartesian_trajectory_planner_B.e_e[0] =
6044 cartesian_trajectory_planner_B.ztest;
6045 }
6046
6047 cartesian_trajectory_planner_B.ztest =
6048 cartesian_trajectory_planner_B.e_e[d_k - 1];
6049 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.s[d_k
6050 - 1];
6051 cartesian_trajectory_planner_B.e_e[d_k - 1] =
6052 cartesian_trajectory_planner_B.ztest *
6053 cartesian_trajectory_planner_B.b_c5 -
6054 cartesian_trajectory_planner_B.rt *
6055 cartesian_trajectory_planner_B.smm1;
6056 cartesian_trajectory_planner_B.sqds =
6057 cartesian_trajectory_planner_B.smm1 *
6058 cartesian_trajectory_planner_B.s[d_k];
6059 cartesian_trajectory_planner_B.s[d_k] *=
6060 cartesian_trajectory_planner_B.b_c5;
6061 qjj = (d_k - 1) * 3 + 1;
6062 kase = 3 * d_k + 1;
6063 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
6064 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, qjj,
6065 kase, cartesian_trajectory_planner_B.b_c5,
6066 cartesian_trajectory_planner_B.smm1, V);
6067 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.rt *
6068 cartesian_trajectory_planner_B.b_c5 +
6069 cartesian_trajectory_planner_B.ztest *
6070 cartesian_trajectory_planner_B.smm1,
6071 cartesian_trajectory_planner_B.sqds,
6072 &cartesian_trajectory_planner_B.s[d_k - 1],
6073 &cartesian_trajectory_planner_B.unusedU2,
6074 &cartesian_trajectory_planner_B.emm1,
6075 &cartesian_trajectory_planner_B.d_sn);
6076 cartesian_trajectory_planner_B.rt =
6077 cartesian_trajectory_planner_B.e_e[d_k - 1] *
6078 cartesian_trajectory_planner_B.emm1 +
6079 cartesian_trajectory_planner_B.d_sn *
6080 cartesian_trajectory_planner_B.s[d_k];
6081 cartesian_trajectory_planner_B.s[d_k] =
6082 cartesian_trajectory_planner_B.e_e[d_k - 1] *
6083 -cartesian_trajectory_planner_B.d_sn +
6084 cartesian_trajectory_planner_B.emm1 *
6085 cartesian_trajectory_planner_B.s[d_k];
6086 cartesian_trajectory_planner_B.sqds =
6087 cartesian_trajectory_planner_B.d_sn *
6088 cartesian_trajectory_planner_B.e_e[d_k];
6089 cartesian_trajectory_planner_B.e_e[d_k] *=
6090 cartesian_trajectory_planner_B.emm1;
6091 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
6092 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, qjj,
6093 kase, cartesian_trajectory_planner_B.emm1,
6094 cartesian_trajectory_planner_B.d_sn, U);
6095 }
6096
6097 cartesian_trajectory_planner_B.e_e[m - 1] =
6098 cartesian_trajectory_planner_B.rt;
6099 qq++;
6100 break;
6101
6102 default:
6103 if (cartesian_trajectory_planner_B.s[c_q] < 0.0) {
6104 cartesian_trajectory_planner_B.s[c_q] =
6105 -cartesian_trajectory_planner_B.s[c_q];
6106 qjj = 3 * c_q;
6107 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
6108 V[qq - 1] = -V[qq - 1];
6109 }
6110 }
6111
6112 qq = c_q + 1;
6113 while ((c_q + 1 < 3) && (cartesian_trajectory_planner_B.s[c_q] <
6114 cartesian_trajectory_planner_B.s[qq])) {
6115 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.s[c_q];
6116 cartesian_trajectory_planner_B.s[c_q] =
6117 cartesian_trajectory_planner_B.s[qq];
6118 cartesian_trajectory_planner_B.s[qq] = cartesian_trajectory_planner_B.rt;
6119 qjj = 3 * c_q + 1;
6120 kase = (c_q + 1) * 3 + 1;
6121 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
6122 cartesian_trajectory_plan_xswap(cartesian_trajectory_planner_B.A, qjj,
6123 kase, V);
6124 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
6125 cartesian_trajectory_plan_xswap(cartesian_trajectory_planner_B.A, qjj,
6126 kase, U);
6127 c_q = qq;
6128 qq++;
6129 }
6130
6131 qq = 0;
6132 m--;
6133 break;
6134 }
6135 }
6136
6137 s[0] = cartesian_trajectory_planner_B.s[0];
6138 s[1] = cartesian_trajectory_planner_B.s[1];
6139 s[2] = cartesian_trajectory_planner_B.s[2];
6140}
6141
6142static void cartesian_trajectory_rotm2axang(const real_T R[9], real_T axang[4])
6143{
6144 boolean_T e;
6145 boolean_T p;
6146 boolean_T rEQ0;
6147 int32_T loop_ub_tmp;
6148 boolean_T exitg1;
6149 cartesian_trajectory_planner_B.u.re = (((R[0] + R[4]) + R[8]) - 1.0) * 0.5;
6150 if (!(fabs(cartesian_trajectory_planner_B.u.re) > 1.0)) {
6151 cartesian_trajectory_planner_B.v_mv.re = acos
6152 (cartesian_trajectory_planner_B.u.re);
6153 } else {
6154 cartesian_trajectory_planner_B.u_m.re = cartesian_trajectory_planner_B.u.re
6155 + 1.0;
6156 cartesian_trajectory_planner_B.u_m.im = 0.0;
6157 cartesian_trajectory_planner_B.dc.re = 1.0 -
6158 cartesian_trajectory_planner_B.u.re;
6159 cartesian_trajectory_planner_B.dc.im = 0.0;
6160 cartesian_trajectory_planner_B.v_mv.re = 2.0 * rt_atan2d_snf
6161 ((cartesian_trajectory_plann_sqrt(cartesian_trajectory_planner_B.dc)).re,
6162 (cartesian_trajectory_plann_sqrt(cartesian_trajectory_planner_B.u_m)).re);
6163 }
6164
6165 cartesian_trajectory_planner_B.a_da = 2.0 * sin
6166 (cartesian_trajectory_planner_B.v_mv.re);
6167 cartesian_trajectory_planner_B.v_j[0] = (R[5] - R[7]) /
6168 cartesian_trajectory_planner_B.a_da;
6169 cartesian_trajectory_planner_B.v_j[1] = (R[6] - R[2]) /
6170 cartesian_trajectory_planner_B.a_da;
6171 cartesian_trajectory_planner_B.v_j[2] = (R[1] - R[3]) /
6172 cartesian_trajectory_planner_B.a_da;
6173 if (rtIsNaN(cartesian_trajectory_planner_B.v_mv.re) || rtIsInf
6174 (cartesian_trajectory_planner_B.v_mv.re)) {
6175 cartesian_trajectory_planner_B.a_da = (rtNaN);
6176 } else if (cartesian_trajectory_planner_B.v_mv.re == 0.0) {
6177 cartesian_trajectory_planner_B.a_da = 0.0;
6178 } else {
6179 cartesian_trajectory_planner_B.a_da = fmod
6180 (cartesian_trajectory_planner_B.v_mv.re, 3.1415926535897931);
6181 rEQ0 = (cartesian_trajectory_planner_B.a_da == 0.0);
6182 if (!rEQ0) {
6183 cartesian_trajectory_planner_B.q = fabs
6184 (cartesian_trajectory_planner_B.v_mv.re / 3.1415926535897931);
6185 rEQ0 = !(fabs(cartesian_trajectory_planner_B.q - floor
6186 (cartesian_trajectory_planner_B.q + 0.5)) >
6187 2.2204460492503131E-16 * cartesian_trajectory_planner_B.q);
6188 }
6189
6190 if (rEQ0) {
6191 cartesian_trajectory_planner_B.a_da = 0.0;
6192 } else {
6193 if (cartesian_trajectory_planner_B.v_mv.re < 0.0) {
6194 cartesian_trajectory_planner_B.a_da += 3.1415926535897931;
6195 }
6196 }
6197 }
6198
6199 rEQ0 = (cartesian_trajectory_planner_B.a_da == 0.0);
6200 e = true;
6201 cartesian_trajectory_planner_B.b_k_o = 0;
6202 exitg1 = false;
6203 while ((!exitg1) && (cartesian_trajectory_planner_B.b_k_o < 3)) {
6204 if (!(cartesian_trajectory_planner_B.v_j[cartesian_trajectory_planner_B.b_k_o]
6205 == 0.0)) {
6206 e = false;
6207 exitg1 = true;
6208 } else {
6209 cartesian_trajectory_planner_B.b_k_o++;
6210 }
6211 }
6212
6213 if (rEQ0 || e) {
6214 loop_ub_tmp = (rEQ0 || e);
6215 cartesian_trajectory_planner_B.loop_ub_iz = loop_ub_tmp * 3 - 1;
6216 if (0 <= cartesian_trajectory_planner_B.loop_ub_iz) {
6217 memset(&cartesian_trajectory_planner_B.vspecial_data[0], 0,
6218 (cartesian_trajectory_planner_B.loop_ub_iz + 1) * sizeof(real_T));
6219 }
6220
6221 loop_ub_tmp--;
6222 for (cartesian_trajectory_planner_B.loop_ub_iz = 0;
6223 cartesian_trajectory_planner_B.loop_ub_iz <= loop_ub_tmp;
6224 cartesian_trajectory_planner_B.loop_ub_iz++) {
6225 memset(&cartesian_trajectory_planner_B.b_I[0], 0, 9U * sizeof(real_T));
6226 cartesian_trajectory_planner_B.b_I[0] = 1.0;
6227 cartesian_trajectory_planner_B.b_I[4] = 1.0;
6228 cartesian_trajectory_planner_B.b_I[8] = 1.0;
6229 p = true;
6230 for (cartesian_trajectory_planner_B.b_k_o = 0;
6231 cartesian_trajectory_planner_B.b_k_o < 9;
6232 cartesian_trajectory_planner_B.b_k_o++) {
6233 cartesian_trajectory_planner_B.a_da =
6234 cartesian_trajectory_planner_B.b_I[cartesian_trajectory_planner_B.b_k_o]
6235 - R[cartesian_trajectory_planner_B.b_k_o];
6236 if (p && ((!rtIsInf(cartesian_trajectory_planner_B.a_da)) && (!rtIsNaN
6237 (cartesian_trajectory_planner_B.a_da)))) {
6238 } else {
6239 p = false;
6240 }
6241
6242 cartesian_trajectory_planner_B.b_I[cartesian_trajectory_planner_B.b_k_o]
6243 = cartesian_trajectory_planner_B.a_da;
6244 }
6245
6246 if (p) {
6247 cartesian_trajectory_planne_svd(cartesian_trajectory_planner_B.b_I,
6248 cartesian_trajectory_planner_B.b_U,
6249 cartesian_trajectory_planner_B.vspecial_data,
6250 cartesian_trajectory_planner_B.V_f);
6251 } else {
6252 for (cartesian_trajectory_planner_B.b_k_o = 0;
6253 cartesian_trajectory_planner_B.b_k_o < 9;
6254 cartesian_trajectory_planner_B.b_k_o++) {
6255 cartesian_trajectory_planner_B.V_f[cartesian_trajectory_planner_B.b_k_o]
6256 = (rtNaN);
6257 }
6258 }
6259
6260 cartesian_trajectory_planner_B.vspecial_data[0] =
6261 cartesian_trajectory_planner_B.V_f[6];
6262 cartesian_trajectory_planner_B.vspecial_data[1] =
6263 cartesian_trajectory_planner_B.V_f[7];
6264 cartesian_trajectory_planner_B.vspecial_data[2] =
6265 cartesian_trajectory_planner_B.V_f[8];
6266 }
6267
6268 loop_ub_tmp = 0;
6269 if (rEQ0 || e) {
6270 for (cartesian_trajectory_planner_B.loop_ub_iz = 0;
6271 cartesian_trajectory_planner_B.loop_ub_iz < 1;
6272 cartesian_trajectory_planner_B.loop_ub_iz++) {
6273 loop_ub_tmp++;
6274 }
6275 }
6276
6277 for (cartesian_trajectory_planner_B.b_k_o = 0;
6278 cartesian_trajectory_planner_B.b_k_o < loop_ub_tmp;
6279 cartesian_trajectory_planner_B.b_k_o++) {
6280 cartesian_trajectory_planner_B.v_j[0] =
6281 cartesian_trajectory_planner_B.vspecial_data[3 *
6282 cartesian_trajectory_planner_B.b_k_o];
6283 cartesian_trajectory_planner_B.v_j[1] =
6284 cartesian_trajectory_planner_B.vspecial_data[3 *
6285 cartesian_trajectory_planner_B.b_k_o + 1];
6286 cartesian_trajectory_planner_B.v_j[2] =
6287 cartesian_trajectory_planner_B.vspecial_data[3 *
6288 cartesian_trajectory_planner_B.b_k_o + 2];
6289 }
6290 }
6291
6292 cartesian_trajectory_planner_B.a_da = 1.0 / sqrt
6293 ((cartesian_trajectory_planner_B.v_j[0] *
6294 cartesian_trajectory_planner_B.v_j[0] +
6295 cartesian_trajectory_planner_B.v_j[1] *
6296 cartesian_trajectory_planner_B.v_j[1]) +
6297 cartesian_trajectory_planner_B.v_j[2] * cartesian_trajectory_planner_B.v_j
6298 [2]);
6299 cartesian_trajectory_planner_B.v_j[0] *= cartesian_trajectory_planner_B.a_da;
6300 cartesian_trajectory_planner_B.v_j[1] *= cartesian_trajectory_planner_B.a_da;
6301 axang[0] = cartesian_trajectory_planner_B.v_j[0];
6302 axang[1] = cartesian_trajectory_planner_B.v_j[1];
6303 axang[2] = cartesian_trajectory_planner_B.v_j[2] *
6304 cartesian_trajectory_planner_B.a_da;
6305 axang[3] = cartesian_trajectory_planner_B.v_mv.re;
6306}
6307
6308static void cartesian_IKHelpers_computeCost(const real_T x[6],
6309 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
6310 emxArray_real_T_cartesian_tra_T *Jac, f_robotics_manip_internal_IKE_T **b_args)
6311{
6312 x_robotics_manip_internal_Rig_T *treeInternal;
6313 emxArray_char_T_cartesian_tra_T *bodyName;
6314 emxArray_real_T_cartesian_tra_T *J;
6315 emxArray_real_T_cartesian_tra_T *y;
6316 cartesian_traj_emxInit_char_T_a(&bodyName, 2);
6317 *b_args = args;
6318 treeInternal = args->Robot;
6319 cartesian_trajectory_planner_B.b_j_k = bodyName->size[0] * bodyName->size[1];
6320 bodyName->size[0] = 1;
6321 bodyName->size[1] = args->BodyName->size[1];
6322 cart_emxEnsureCapacity_char_T_a(bodyName, cartesian_trajectory_planner_B.b_j_k);
6323 cartesian_trajectory_planner_B.loop_ub_j = args->BodyName->size[0] *
6324 args->BodyName->size[1] - 1;
6325 for (cartesian_trajectory_planner_B.b_j_k = 0;
6326 cartesian_trajectory_planner_B.b_j_k <=
6327 cartesian_trajectory_planner_B.loop_ub_j;
6328 cartesian_trajectory_planner_B.b_j_k++) {
6329 bodyName->data[cartesian_trajectory_planner_B.b_j_k] = args->BodyName->
6330 data[cartesian_trajectory_planner_B.b_j_k];
6331 }
6332
6333 for (cartesian_trajectory_planner_B.b_j_k = 0;
6334 cartesian_trajectory_planner_B.b_j_k < 16;
6335 cartesian_trajectory_planner_B.b_j_k++) {
6336 cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.b_j_k] =
6337 args->Tform[cartesian_trajectory_planner_B.b_j_k];
6338 }
6339
6340 for (cartesian_trajectory_planner_B.b_j_k = 0;
6341 cartesian_trajectory_planner_B.b_j_k < 36;
6342 cartesian_trajectory_planner_B.b_j_k++) {
6343 W[cartesian_trajectory_planner_B.b_j_k] = args->
6344 WeightMatrix[cartesian_trajectory_planner_B.b_j_k];
6345 }
6346
6347 cartesian_trajec_emxInit_real_T(&J, 2);
6348 RigidBodyTree_efficientFKAndJac(treeInternal, x, bodyName,
6349 cartesian_trajectory_planner_B.T_data, cartesian_trajectory_planner_B.T_size,
6350 J);
6351 cartesian_trajectory_planner_B.b_j_k = Jac->size[0] * Jac->size[1];
6352 Jac->size[0] = 6;
6353 Jac->size[1] = J->size[1];
6354 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_j_k);
6355 cartesian_trajectory_planner_B.loop_ub_j = J->size[0] * J->size[1] - 1;
6356 cartesian_traj_emxFree_char_T_a(&bodyName);
6357 for (cartesian_trajectory_planner_B.b_j_k = 0;
6358 cartesian_trajectory_planner_B.b_j_k <=
6359 cartesian_trajectory_planner_B.loop_ub_j;
6360 cartesian_trajectory_planner_B.b_j_k++) {
6361 Jac->data[cartesian_trajectory_planner_B.b_j_k] = -J->
6362 data[cartesian_trajectory_planner_B.b_j_k];
6363 }
6364
6365 cartesian_trajec_emxFree_real_T(&J);
6366 for (cartesian_trajectory_planner_B.b_j_k = 0;
6367 cartesian_trajectory_planner_B.b_j_k < 3;
6368 cartesian_trajectory_planner_B.b_j_k++) {
6369 cartesian_trajectory_planner_B.T_l[3 * cartesian_trajectory_planner_B.b_j_k]
6370 =
6371 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.b_j_k];
6372 cartesian_trajectory_planner_B.n_e = 3 *
6373 cartesian_trajectory_planner_B.b_j_k + 1;
6374 cartesian_trajectory_planner_B.T_l[cartesian_trajectory_planner_B.n_e] =
6375 cartesian_trajectory_planner_B.T_data
6376 [((cartesian_trajectory_planner_B.b_j_k + 1) +
6377 cartesian_trajectory_planner_B.T_size[0]) - 1];
6378 cartesian_trajectory_planner_B.boffset_h = 3 *
6379 cartesian_trajectory_planner_B.b_j_k + 2;
6380 cartesian_trajectory_planner_B.T_l[cartesian_trajectory_planner_B.boffset_h]
6381 = cartesian_trajectory_planner_B.T_data
6382 [((cartesian_trajectory_planner_B.b_j_k + 1) +
6383 (cartesian_trajectory_planner_B.T_size[0] << 1)) - 1];
6384 for (cartesian_trajectory_planner_B.loop_ub_j = 0;
6385 cartesian_trajectory_planner_B.loop_ub_j < 3;
6386 cartesian_trajectory_planner_B.loop_ub_j++) {
6387 cartesian_trajectory_planner_B.Td_tmp =
6388 cartesian_trajectory_planner_B.loop_ub_j + 3 *
6389 cartesian_trajectory_planner_B.b_j_k;
6390 cartesian_trajectory_planner_B.Td_o[cartesian_trajectory_planner_B.Td_tmp]
6391 = 0.0;
6392 cartesian_trajectory_planner_B.Td_o[cartesian_trajectory_planner_B.Td_tmp]
6393 += cartesian_trajectory_planner_B.T_l[3 *
6394 cartesian_trajectory_planner_B.b_j_k] *
6395 cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.loop_ub_j];
6396 cartesian_trajectory_planner_B.Td_o[cartesian_trajectory_planner_B.Td_tmp]
6397 += cartesian_trajectory_planner_B.T_l[cartesian_trajectory_planner_B.n_e]
6398 * cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.loop_ub_j
6399 + 4];
6400 cartesian_trajectory_planner_B.Td_o[cartesian_trajectory_planner_B.Td_tmp]
6401 +=
6402 cartesian_trajectory_planner_B.T_l[cartesian_trajectory_planner_B.boffset_h]
6403 * cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.loop_ub_j
6404 + 8];
6405 }
6406 }
6407
6408 cartesian_trajectory_rotm2axang(cartesian_trajectory_planner_B.Td_o,
6409 cartesian_trajectory_planner_B.v);
6410 cartesian_trajectory_planner_B.e[0] = cartesian_trajectory_planner_B.v[3] *
6411 cartesian_trajectory_planner_B.v[0];
6412 cartesian_trajectory_planner_B.e[3] = cartesian_trajectory_planner_B.Td[12] -
6413 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.T_size
6414 [0] * 3];
6415 cartesian_trajectory_planner_B.e[1] = cartesian_trajectory_planner_B.v[3] *
6416 cartesian_trajectory_planner_B.v[1];
6417 cartesian_trajectory_planner_B.e[4] = cartesian_trajectory_planner_B.Td[13] -
6418 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.T_size
6419 [0] * 3 + 1];
6420 cartesian_trajectory_planner_B.e[2] = cartesian_trajectory_planner_B.v[3] *
6421 cartesian_trajectory_planner_B.v[2];
6422 cartesian_trajectory_planner_B.e[5] = cartesian_trajectory_planner_B.Td[14] -
6423 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.T_size
6424 [0] * 3 + 2];
6425 cartesian_trajectory_planner_B.b_j_k = args->ErrTemp->size[0];
6426 args->ErrTemp->size[0] = 6;
6427 cartes_emxEnsureCapacity_real_T(args->ErrTemp,
6428 cartesian_trajectory_planner_B.b_j_k);
6429 for (cartesian_trajectory_planner_B.b_j_k = 0;
6430 cartesian_trajectory_planner_B.b_j_k < 6;
6431 cartesian_trajectory_planner_B.b_j_k++) {
6432 args->ErrTemp->data[cartesian_trajectory_planner_B.b_j_k] =
6433 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.b_j_k];
6434 }
6435
6436 for (cartesian_trajectory_planner_B.b_j_k = 0;
6437 cartesian_trajectory_planner_B.b_j_k < 6;
6438 cartesian_trajectory_planner_B.b_j_k++) {
6439 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_k] = 0.0;
6440 for (cartesian_trajectory_planner_B.loop_ub_j = 0;
6441 cartesian_trajectory_planner_B.loop_ub_j < 6;
6442 cartesian_trajectory_planner_B.loop_ub_j++) {
6443 cartesian_trajectory_planner_B.s_l = W[6 *
6444 cartesian_trajectory_planner_B.b_j_k +
6445 cartesian_trajectory_planner_B.loop_ub_j] * (0.5 *
6446 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.loop_ub_j])
6447 + cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_k];
6448 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_k] =
6449 cartesian_trajectory_planner_B.s_l;
6450 }
6451 }
6452
6453 cartesian_trajectory_planner_B.s_l = 0.0;
6454 for (cartesian_trajectory_planner_B.b_j_k = 0;
6455 cartesian_trajectory_planner_B.b_j_k < 6;
6456 cartesian_trajectory_planner_B.b_j_k++) {
6457 cartesian_trajectory_planner_B.s_l +=
6458 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_k] *
6459 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.b_j_k];
6460 }
6461
6462 args->CostTemp = cartesian_trajectory_planner_B.s_l;
6463 for (cartesian_trajectory_planner_B.b_j_k = 0;
6464 cartesian_trajectory_planner_B.b_j_k < 6;
6465 cartesian_trajectory_planner_B.b_j_k++) {
6466 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_k] = 0.0;
6467 for (cartesian_trajectory_planner_B.loop_ub_j = 0;
6468 cartesian_trajectory_planner_B.loop_ub_j < 6;
6469 cartesian_trajectory_planner_B.loop_ub_j++) {
6470 cartesian_trajectory_planner_B.s_l = W[6 *
6471 cartesian_trajectory_planner_B.b_j_k +
6472 cartesian_trajectory_planner_B.loop_ub_j] *
6473 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.loop_ub_j]
6474 + cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_k];
6475 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_k] =
6476 cartesian_trajectory_planner_B.s_l;
6477 }
6478 }
6479
6480 cartesian_trajec_emxInit_real_T(&y, 2);
6481 cartesian_trajectory_planner_B.n_e = Jac->size[1] - 1;
6482 cartesian_trajectory_planner_B.b_j_k = y->size[0] * y->size[1];
6483 y->size[0] = 1;
6484 y->size[1] = Jac->size[1];
6485 cartes_emxEnsureCapacity_real_T(y, cartesian_trajectory_planner_B.b_j_k);
6486 for (cartesian_trajectory_planner_B.b_j_k = 0;
6487 cartesian_trajectory_planner_B.b_j_k <=
6488 cartesian_trajectory_planner_B.n_e; cartesian_trajectory_planner_B.b_j_k
6489 ++) {
6490 cartesian_trajectory_planner_B.boffset_h =
6491 cartesian_trajectory_planner_B.b_j_k * 6 - 1;
6492 cartesian_trajectory_planner_B.s_l = 0.0;
6493 for (cartesian_trajectory_planner_B.loop_ub_j = 0;
6494 cartesian_trajectory_planner_B.loop_ub_j < 6;
6495 cartesian_trajectory_planner_B.loop_ub_j++) {
6496 cartesian_trajectory_planner_B.s_l += Jac->data
6497 [(cartesian_trajectory_planner_B.boffset_h +
6498 cartesian_trajectory_planner_B.loop_ub_j) + 1] *
6499 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.loop_ub_j];
6500 }
6501
6502 y->data[cartesian_trajectory_planner_B.b_j_k] =
6503 cartesian_trajectory_planner_B.s_l;
6504 }
6505
6506 cartesian_trajectory_planner_B.b_j_k = args->GradTemp->size[0];
6507 args->GradTemp->size[0] = y->size[1];
6508 cartes_emxEnsureCapacity_real_T(args->GradTemp,
6509 cartesian_trajectory_planner_B.b_j_k);
6510 cartesian_trajectory_planner_B.loop_ub_j = y->size[1];
6511 for (cartesian_trajectory_planner_B.b_j_k = 0;
6512 cartesian_trajectory_planner_B.b_j_k <
6513 cartesian_trajectory_planner_B.loop_ub_j;
6514 cartesian_trajectory_planner_B.b_j_k++) {
6515 args->GradTemp->data[cartesian_trajectory_planner_B.b_j_k] = y->
6516 data[cartesian_trajectory_planner_B.b_j_k];
6517 }
6518
6519 cartesian_trajec_emxFree_real_T(&y);
6520 cartesian_trajectory_planner_B.s_l = args->CostTemp;
6521 *cost = cartesian_trajectory_planner_B.s_l;
6522}
6523
6524static void cartesian_trajectory_planne_eye(real_T b_I[36])
6525{
6526 int32_T b_k;
6527 memset(&b_I[0], 0, 36U * sizeof(real_T));
6528 for (b_k = 0; b_k < 6; b_k++) {
6529 b_I[b_k + 6 * b_k] = 1.0;
6530 }
6531}
6532
6533static void cartesian_tra_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
6534 **pEmxArray, int32_T numDimensions)
6535{
6536 emxArray_boolean_T_cartesian__T *emxArray;
6537 int32_T i;
6538 *pEmxArray = (emxArray_boolean_T_cartesian__T *)malloc(sizeof
6539 (emxArray_boolean_T_cartesian__T));
6540 emxArray = *pEmxArray;
6541 emxArray->data = (boolean_T *)NULL;
6542 emxArray->numDimensions = numDimensions;
6543 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
6544 emxArray->allocatedSize = 0;
6545 emxArray->canFreeData = true;
6546 for (i = 0; i < numDimensions; i++) {
6547 emxArray->size[i] = 0;
6548 }
6549}
6550
6551static void cartesian_traje_emxInit_int32_T(emxArray_int32_T_cartesian_tr_T
6552 **pEmxArray, int32_T numDimensions)
6553{
6554 emxArray_int32_T_cartesian_tr_T *emxArray;
6555 int32_T i;
6556 *pEmxArray = (emxArray_int32_T_cartesian_tr_T *)malloc(sizeof
6557 (emxArray_int32_T_cartesian_tr_T));
6558 emxArray = *pEmxArray;
6559 emxArray->data = (int32_T *)NULL;
6560 emxArray->numDimensions = numDimensions;
6561 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
6562 emxArray->allocatedSize = 0;
6563 emxArray->canFreeData = true;
6564 for (i = 0; i < numDimensions; i++) {
6565 emxArray->size[i] = 0;
6566 }
6567}
6568
6569static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
6570 *emxArray, int32_T oldNumel)
6571{
6572 int32_T newNumel;
6573 int32_T i;
6574 void *newData;
6575 if (oldNumel < 0) {
6576 oldNumel = 0;
6577 }
6578
6579 newNumel = 1;
6580 for (i = 0; i < emxArray->numDimensions; i++) {
6581 newNumel *= emxArray->size[i];
6582 }
6583
6584 if (newNumel > emxArray->allocatedSize) {
6585 i = emxArray->allocatedSize;
6586 if (i < 16) {
6587 i = 16;
6588 }
6589
6590 while (i < newNumel) {
6591 if (i > 1073741823) {
6592 i = MAX_int32_T;
6593 } else {
6594 i <<= 1;
6595 }
6596 }
6597
6598 newData = calloc(static_cast<uint32_T>(i), sizeof(boolean_T));
6599 if (emxArray->data != NULL) {
6600 memcpy(newData, emxArray->data, sizeof(boolean_T) * oldNumel);
6601 if (emxArray->canFreeData) {
6602 free(emxArray->data);
6603 }
6604 }
6605
6606 emxArray->data = (boolean_T *)newData;
6607 emxArray->allocatedSize = i;
6608 emxArray->canFreeData = true;
6609 }
6610}
6611
6612static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_tr_T
6613 *emxArray, int32_T oldNumel)
6614{
6615 int32_T newNumel;
6616 int32_T i;
6617 void *newData;
6618 if (oldNumel < 0) {
6619 oldNumel = 0;
6620 }
6621
6622 newNumel = 1;
6623 for (i = 0; i < emxArray->numDimensions; i++) {
6624 newNumel *= emxArray->size[i];
6625 }
6626
6627 if (newNumel > emxArray->allocatedSize) {
6628 i = emxArray->allocatedSize;
6629 if (i < 16) {
6630 i = 16;
6631 }
6632
6633 while (i < newNumel) {
6634 if (i > 1073741823) {
6635 i = MAX_int32_T;
6636 } else {
6637 i <<= 1;
6638 }
6639 }
6640
6641 newData = calloc(static_cast<uint32_T>(i), sizeof(int32_T));
6642 if (emxArray->data != NULL) {
6643 memcpy(newData, emxArray->data, sizeof(int32_T) * oldNumel);
6644 if (emxArray->canFreeData) {
6645 free(emxArray->data);
6646 }
6647 }
6648
6649 emxArray->data = (int32_T *)newData;
6650 emxArray->allocatedSize = i;
6651 emxArray->canFreeData = true;
6652 }
6653}
6654
6655static real_T cartesian_trajectory_pla_norm_a(const real_T x[6])
6656{
6657 real_T y;
6658 real_T scale;
6659 real_T absxk;
6660 real_T t;
6661 int32_T b_k;
6662 y = 0.0;
6663 scale = 3.3121686421112381E-170;
6664 for (b_k = 0; b_k < 6; b_k++) {
6665 absxk = fabs(x[b_k]);
6666 if (absxk > scale) {
6667 t = scale / absxk;
6668 y = y * t * t + 1.0;
6669 scale = absxk;
6670 } else {
6671 t = absxk / scale;
6672 y += t * t;
6673 }
6674 }
6675
6676 return scale * sqrt(y);
6677}
6678
6679static real_T SystemTimeProvider_getElapsedTi(const
6680 f_robotics_core_internal_Syst_T *obj)
6681{
6682 real_T systemTime;
6683 systemTime = ctimefun();
6684 return systemTime - obj->StartTime;
6685}
6686
6687static real_T cartesian_trajectory_p_xnrm2_as(int32_T n, const
6688 emxArray_real_T_cartesian_tra_T *x, int32_T ix0)
6689{
6690 real_T y;
6691 y = 0.0;
6692 if (n >= 1) {
6693 if (n == 1) {
6694 y = fabs(x->data[ix0 - 1]);
6695 } else {
6696 cartesian_trajectory_planner_B.scale_k = 3.3121686421112381E-170;
6697 cartesian_trajectory_planner_B.kend_j = ix0 + n;
6698 for (cartesian_trajectory_planner_B.k_np = ix0;
6699 cartesian_trajectory_planner_B.k_np <
6700 cartesian_trajectory_planner_B.kend_j;
6701 cartesian_trajectory_planner_B.k_np++) {
6702 cartesian_trajectory_planner_B.absxk_f = fabs(x->
6703 data[cartesian_trajectory_planner_B.k_np - 1]);
6704 if (cartesian_trajectory_planner_B.absxk_f >
6705 cartesian_trajectory_planner_B.scale_k) {
6706 cartesian_trajectory_planner_B.t_a =
6707 cartesian_trajectory_planner_B.scale_k /
6708 cartesian_trajectory_planner_B.absxk_f;
6709 y = y * cartesian_trajectory_planner_B.t_a *
6710 cartesian_trajectory_planner_B.t_a + 1.0;
6711 cartesian_trajectory_planner_B.scale_k =
6712 cartesian_trajectory_planner_B.absxk_f;
6713 } else {
6714 cartesian_trajectory_planner_B.t_a =
6715 cartesian_trajectory_planner_B.absxk_f /
6716 cartesian_trajectory_planner_B.scale_k;
6717 y += cartesian_trajectory_planner_B.t_a *
6718 cartesian_trajectory_planner_B.t_a;
6719 }
6720 }
6721
6722 y = cartesian_trajectory_planner_B.scale_k * sqrt(y);
6723 }
6724 }
6725
6726 return y;
6727}
6728
6729static void cartesian_trajectory_pla_qrpf_a(const
6730 emxArray_real_T_cartesian_tra_T *A, int32_T m, int32_T n,
6731 emxArray_real_T_cartesian_tra_T *tau, const emxArray_int32_T_cartesian_tr_T
6732 *jpvt, emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T
6733 *b_jpvt)
6734{
6735 emxArray_real_T_cartesian_tra_T *work;
6736 emxArray_real_T_cartesian_tra_T *vn1;
6737 emxArray_real_T_cartesian_tra_T *vn2;
6738 emxArray_real_T_cartesian_tra_T *c_x;
6739 int32_T exitg1;
6740 boolean_T exitg2;
6741 cartesian_trajectory_planner_B.kend = b_jpvt->size[0] * b_jpvt->size[1];
6742 b_jpvt->size[0] = 1;
6743 b_jpvt->size[1] = jpvt->size[1];
6744 carte_emxEnsureCapacity_int32_T(b_jpvt, cartesian_trajectory_planner_B.kend);
6745 cartesian_trajectory_planner_B.ix_c = jpvt->size[0] * jpvt->size[1] - 1;
6746 for (cartesian_trajectory_planner_B.kend = 0;
6747 cartesian_trajectory_planner_B.kend <=
6748 cartesian_trajectory_planner_B.ix_c; cartesian_trajectory_planner_B.kend
6749 ++) {
6750 b_jpvt->data[cartesian_trajectory_planner_B.kend] = jpvt->
6751 data[cartesian_trajectory_planner_B.kend];
6752 }
6753
6754 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
6755 b_A->size[0] = A->size[0];
6756 b_A->size[1] = A->size[1];
6757 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
6758 cartesian_trajectory_planner_B.ix_c = A->size[0] * A->size[1] - 1;
6759 for (cartesian_trajectory_planner_B.kend = 0;
6760 cartesian_trajectory_planner_B.kend <=
6761 cartesian_trajectory_planner_B.ix_c; cartesian_trajectory_planner_B.kend
6762 ++) {
6763 b_A->data[cartesian_trajectory_planner_B.kend] = A->
6764 data[cartesian_trajectory_planner_B.kend];
6765 }
6766
6767 cartesian_trajec_emxInit_real_T(&work, 1);
6768 cartesian_trajectory_planner_B.ma = A->size[0];
6769 if (m < n) {
6770 cartesian_trajectory_planner_B.m_d1 = m;
6771 } else {
6772 cartesian_trajectory_planner_B.m_d1 = n;
6773 }
6774
6775 cartesian_trajectory_planner_B.minmn_f = cartesian_trajectory_planner_B.m_d1 -
6776 1;
6777 cartesian_trajectory_planner_B.kend = work->size[0];
6778 work->size[0] = A->size[1];
6779 cartes_emxEnsureCapacity_real_T(work, cartesian_trajectory_planner_B.kend);
6780 cartesian_trajectory_planner_B.ix_c = A->size[1];
6781 for (cartesian_trajectory_planner_B.kend = 0;
6782 cartesian_trajectory_planner_B.kend < cartesian_trajectory_planner_B.ix_c;
6783 cartesian_trajectory_planner_B.kend++) {
6784 work->data[cartesian_trajectory_planner_B.kend] = 0.0;
6785 }
6786
6787 cartesian_trajec_emxInit_real_T(&vn1, 1);
6788 cartesian_trajectory_planner_B.kend = vn1->size[0];
6789 vn1->size[0] = A->size[1];
6790 cartes_emxEnsureCapacity_real_T(vn1, cartesian_trajectory_planner_B.kend);
6791 cartesian_trajectory_planner_B.ix_c = A->size[1];
6792 for (cartesian_trajectory_planner_B.kend = 0;
6793 cartesian_trajectory_planner_B.kend < cartesian_trajectory_planner_B.ix_c;
6794 cartesian_trajectory_planner_B.kend++) {
6795 vn1->data[cartesian_trajectory_planner_B.kend] = 0.0;
6796 }
6797
6798 cartesian_trajec_emxInit_real_T(&vn2, 1);
6799 cartesian_trajectory_planner_B.kend = vn2->size[0];
6800 vn2->size[0] = A->size[1];
6801 cartes_emxEnsureCapacity_real_T(vn2, cartesian_trajectory_planner_B.kend);
6802 cartesian_trajectory_planner_B.ix_c = A->size[1];
6803 for (cartesian_trajectory_planner_B.kend = 0;
6804 cartesian_trajectory_planner_B.kend < cartesian_trajectory_planner_B.ix_c;
6805 cartesian_trajectory_planner_B.kend++) {
6806 vn2->data[cartesian_trajectory_planner_B.kend] = 0.0;
6807 }
6808
6809 for (cartesian_trajectory_planner_B.m_d1 = 0;
6810 cartesian_trajectory_planner_B.m_d1 < n;
6811 cartesian_trajectory_planner_B.m_d1++) {
6812 cartesian_trajectory_planner_B.pvt = cartesian_trajectory_planner_B.m_d1 *
6813 cartesian_trajectory_planner_B.ma;
6814 cartesian_trajectory_planner_B.smax = 0.0;
6815 if (m >= 1) {
6816 if (m == 1) {
6817 cartesian_trajectory_planner_B.smax = fabs(A->
6818 data[cartesian_trajectory_planner_B.pvt]);
6819 } else {
6820 cartesian_trajectory_planner_B.scale_i = 3.3121686421112381E-170;
6821 cartesian_trajectory_planner_B.kend = cartesian_trajectory_planner_B.pvt
6822 + m;
6823 for (cartesian_trajectory_planner_B.itemp =
6824 cartesian_trajectory_planner_B.pvt + 1;
6825 cartesian_trajectory_planner_B.itemp <=
6826 cartesian_trajectory_planner_B.kend;
6827 cartesian_trajectory_planner_B.itemp++) {
6828 cartesian_trajectory_planner_B.absxk = fabs(A->
6829 data[cartesian_trajectory_planner_B.itemp - 1]);
6830 if (cartesian_trajectory_planner_B.absxk >
6831 cartesian_trajectory_planner_B.scale_i) {
6832 cartesian_trajectory_planner_B.t =
6833 cartesian_trajectory_planner_B.scale_i /
6834 cartesian_trajectory_planner_B.absxk;
6835 cartesian_trajectory_planner_B.smax =
6836 cartesian_trajectory_planner_B.smax *
6837 cartesian_trajectory_planner_B.t *
6838 cartesian_trajectory_planner_B.t + 1.0;
6839 cartesian_trajectory_planner_B.scale_i =
6840 cartesian_trajectory_planner_B.absxk;
6841 } else {
6842 cartesian_trajectory_planner_B.t =
6843 cartesian_trajectory_planner_B.absxk /
6844 cartesian_trajectory_planner_B.scale_i;
6845 cartesian_trajectory_planner_B.smax +=
6846 cartesian_trajectory_planner_B.t *
6847 cartesian_trajectory_planner_B.t;
6848 }
6849 }
6850
6851 cartesian_trajectory_planner_B.smax =
6852 cartesian_trajectory_planner_B.scale_i * sqrt
6853 (cartesian_trajectory_planner_B.smax);
6854 }
6855 }
6856
6857 vn1->data[cartesian_trajectory_planner_B.m_d1] =
6858 cartesian_trajectory_planner_B.smax;
6859 vn2->data[cartesian_trajectory_planner_B.m_d1] = vn1->
6860 data[cartesian_trajectory_planner_B.m_d1];
6861 }
6862
6863 cartesian_trajec_emxInit_real_T(&c_x, 2);
6864 for (cartesian_trajectory_planner_B.m_d1 = 0;
6865 cartesian_trajectory_planner_B.m_d1 <=
6866 cartesian_trajectory_planner_B.minmn_f;
6867 cartesian_trajectory_planner_B.m_d1++) {
6868 cartesian_trajectory_planner_B.iy_j = cartesian_trajectory_planner_B.m_d1 *
6869 cartesian_trajectory_planner_B.ma;
6870 cartesian_trajectory_planner_B.ii = cartesian_trajectory_planner_B.iy_j +
6871 cartesian_trajectory_planner_B.m_d1;
6872 cartesian_trajectory_planner_B.nmi = n - cartesian_trajectory_planner_B.m_d1;
6873 cartesian_trajectory_planner_B.mmi = (m -
6874 cartesian_trajectory_planner_B.m_d1) - 1;
6875 if (cartesian_trajectory_planner_B.nmi < 1) {
6876 cartesian_trajectory_planner_B.kend = 0;
6877 } else {
6878 cartesian_trajectory_planner_B.kend = 1;
6879 if (cartesian_trajectory_planner_B.nmi > 1) {
6880 cartesian_trajectory_planner_B.ix_c =
6881 cartesian_trajectory_planner_B.m_d1;
6882 cartesian_trajectory_planner_B.smax = fabs(vn1->
6883 data[cartesian_trajectory_planner_B.m_d1]);
6884 for (cartesian_trajectory_planner_B.itemp = 2;
6885 cartesian_trajectory_planner_B.itemp <=
6886 cartesian_trajectory_planner_B.nmi;
6887 cartesian_trajectory_planner_B.itemp++) {
6888 cartesian_trajectory_planner_B.ix_c++;
6889 cartesian_trajectory_planner_B.scale_i = fabs(vn1->
6890 data[cartesian_trajectory_planner_B.ix_c]);
6891 if (cartesian_trajectory_planner_B.scale_i >
6892 cartesian_trajectory_planner_B.smax) {
6893 cartesian_trajectory_planner_B.kend =
6894 cartesian_trajectory_planner_B.itemp;
6895 cartesian_trajectory_planner_B.smax =
6896 cartesian_trajectory_planner_B.scale_i;
6897 }
6898 }
6899 }
6900 }
6901
6902 cartesian_trajectory_planner_B.pvt = (cartesian_trajectory_planner_B.m_d1 +
6903 cartesian_trajectory_planner_B.kend) - 1;
6904 if (cartesian_trajectory_planner_B.pvt + 1 !=
6905 cartesian_trajectory_planner_B.m_d1 + 1) {
6906 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
6907 c_x->size[0] = b_A->size[0];
6908 c_x->size[1] = b_A->size[1];
6909 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
6910 cartesian_trajectory_planner_B.ix_c = b_A->size[0] * b_A->size[1] - 1;
6911 for (cartesian_trajectory_planner_B.kend = 0;
6912 cartesian_trajectory_planner_B.kend <=
6913 cartesian_trajectory_planner_B.ix_c;
6914 cartesian_trajectory_planner_B.kend++) {
6915 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
6916 data[cartesian_trajectory_planner_B.kend];
6917 }
6918
6919 cartesian_trajectory_planner_B.ix_c = cartesian_trajectory_planner_B.pvt *
6920 cartesian_trajectory_planner_B.ma;
6921 for (cartesian_trajectory_planner_B.itemp = 0;
6922 cartesian_trajectory_planner_B.itemp < m;
6923 cartesian_trajectory_planner_B.itemp++) {
6924 cartesian_trajectory_planner_B.scale_i = c_x->
6925 data[cartesian_trajectory_planner_B.ix_c];
6926 c_x->data[cartesian_trajectory_planner_B.ix_c] = c_x->
6927 data[cartesian_trajectory_planner_B.iy_j];
6928 c_x->data[cartesian_trajectory_planner_B.iy_j] =
6929 cartesian_trajectory_planner_B.scale_i;
6930 cartesian_trajectory_planner_B.ix_c++;
6931 cartesian_trajectory_planner_B.iy_j++;
6932 }
6933
6934 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
6935 b_A->size[0] = c_x->size[0];
6936 b_A->size[1] = c_x->size[1];
6937 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
6938 cartesian_trajectory_planner_B.ix_c = c_x->size[0] * c_x->size[1] - 1;
6939 for (cartesian_trajectory_planner_B.kend = 0;
6940 cartesian_trajectory_planner_B.kend <=
6941 cartesian_trajectory_planner_B.ix_c;
6942 cartesian_trajectory_planner_B.kend++) {
6943 b_A->data[cartesian_trajectory_planner_B.kend] = c_x->
6944 data[cartesian_trajectory_planner_B.kend];
6945 }
6946
6947 cartesian_trajectory_planner_B.itemp = b_jpvt->
6948 data[cartesian_trajectory_planner_B.pvt];
6949 b_jpvt->data[cartesian_trajectory_planner_B.pvt] = b_jpvt->
6950 data[cartesian_trajectory_planner_B.m_d1];
6951 b_jpvt->data[cartesian_trajectory_planner_B.m_d1] =
6952 cartesian_trajectory_planner_B.itemp;
6953 vn1->data[cartesian_trajectory_planner_B.pvt] = vn1->
6954 data[cartesian_trajectory_planner_B.m_d1];
6955 vn2->data[cartesian_trajectory_planner_B.pvt] = vn2->
6956 data[cartesian_trajectory_planner_B.m_d1];
6957 }
6958
6959 if (cartesian_trajectory_planner_B.m_d1 + 1 < m) {
6960 cartesian_trajectory_planner_B.pvt = cartesian_trajectory_planner_B.ii + 2;
6961 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
6962 c_x->size[0] = b_A->size[0];
6963 c_x->size[1] = b_A->size[1];
6964 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
6965 cartesian_trajectory_planner_B.ix_c = b_A->size[0] * b_A->size[1] - 1;
6966 for (cartesian_trajectory_planner_B.kend = 0;
6967 cartesian_trajectory_planner_B.kend <=
6968 cartesian_trajectory_planner_B.ix_c;
6969 cartesian_trajectory_planner_B.kend++) {
6970 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
6971 data[cartesian_trajectory_planner_B.kend];
6972 }
6973
6974 cartesian_trajectory_planner_B.smax = b_A->
6975 data[cartesian_trajectory_planner_B.ii];
6976 tau->data[cartesian_trajectory_planner_B.m_d1] = 0.0;
6977 if (cartesian_trajectory_planner_B.mmi + 1 > 0) {
6978 cartesian_trajectory_planner_B.scale_i = cartesian_trajectory_p_xnrm2_as
6979 (cartesian_trajectory_planner_B.mmi, b_A,
6980 cartesian_trajectory_planner_B.ii + 2);
6981 if (cartesian_trajectory_planner_B.scale_i != 0.0) {
6982 cartesian_trajectory_planner_B.scale_i = rt_hypotd_snf(b_A->
6983 data[cartesian_trajectory_planner_B.ii],
6984 cartesian_trajectory_planner_B.scale_i);
6985 if (b_A->data[cartesian_trajectory_planner_B.ii] >= 0.0) {
6986 cartesian_trajectory_planner_B.scale_i =
6987 -cartesian_trajectory_planner_B.scale_i;
6988 }
6989
6990 if (fabs(cartesian_trajectory_planner_B.scale_i) <
6991 1.0020841800044864E-292) {
6992 cartesian_trajectory_planner_B.kend = -1;
6993 cartesian_trajectory_planner_B.ix_c =
6994 (cartesian_trajectory_planner_B.ii +
6995 cartesian_trajectory_planner_B.mmi) + 1;
6996 do {
6997 cartesian_trajectory_planner_B.kend++;
6998 for (cartesian_trajectory_planner_B.itemp =
6999 cartesian_trajectory_planner_B.pvt;
7000 cartesian_trajectory_planner_B.itemp <=
7001 cartesian_trajectory_planner_B.ix_c;
7002 cartesian_trajectory_planner_B.itemp++) {
7003 c_x->data[cartesian_trajectory_planner_B.itemp - 1] *=
7004 9.9792015476736E+291;
7005 }
7006
7007 cartesian_trajectory_planner_B.scale_i *= 9.9792015476736E+291;
7008 cartesian_trajectory_planner_B.smax *= 9.9792015476736E+291;
7009 } while (!(fabs(cartesian_trajectory_planner_B.scale_i) >=
7010 1.0020841800044864E-292));
7011
7012 cartesian_trajectory_planner_B.scale_i = rt_hypotd_snf
7013 (cartesian_trajectory_planner_B.smax,
7014 cartesian_trajectory_p_xnrm2_as
7015 (cartesian_trajectory_planner_B.mmi, c_x,
7016 cartesian_trajectory_planner_B.ii + 2));
7017 if (cartesian_trajectory_planner_B.smax >= 0.0) {
7018 cartesian_trajectory_planner_B.scale_i =
7019 -cartesian_trajectory_planner_B.scale_i;
7020 }
7021
7022 tau->data[cartesian_trajectory_planner_B.m_d1] =
7023 (cartesian_trajectory_planner_B.scale_i -
7024 cartesian_trajectory_planner_B.smax) /
7025 cartesian_trajectory_planner_B.scale_i;
7026 cartesian_trajectory_planner_B.smax = 1.0 /
7027 (cartesian_trajectory_planner_B.smax -
7028 cartesian_trajectory_planner_B.scale_i);
7029 for (cartesian_trajectory_planner_B.itemp =
7030 cartesian_trajectory_planner_B.pvt;
7031 cartesian_trajectory_planner_B.itemp <=
7032 cartesian_trajectory_planner_B.ix_c;
7033 cartesian_trajectory_planner_B.itemp++) {
7034 c_x->data[cartesian_trajectory_planner_B.itemp - 1] *=
7035 cartesian_trajectory_planner_B.smax;
7036 }
7037
7038 for (cartesian_trajectory_planner_B.itemp = 0;
7039 cartesian_trajectory_planner_B.itemp <=
7040 cartesian_trajectory_planner_B.kend;
7041 cartesian_trajectory_planner_B.itemp++) {
7042 cartesian_trajectory_planner_B.scale_i *= 1.0020841800044864E-292;
7043 }
7044
7045 cartesian_trajectory_planner_B.smax =
7046 cartesian_trajectory_planner_B.scale_i;
7047 } else {
7048 tau->data[cartesian_trajectory_planner_B.m_d1] =
7049 (cartesian_trajectory_planner_B.scale_i - b_A->
7050 data[cartesian_trajectory_planner_B.ii]) /
7051 cartesian_trajectory_planner_B.scale_i;
7052 cartesian_trajectory_planner_B.smax = 1.0 / (b_A->
7053 data[cartesian_trajectory_planner_B.ii] -
7054 cartesian_trajectory_planner_B.scale_i);
7055 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
7056 c_x->size[0] = b_A->size[0];
7057 c_x->size[1] = b_A->size[1];
7058 cartes_emxEnsureCapacity_real_T(c_x,
7059 cartesian_trajectory_planner_B.kend);
7060 cartesian_trajectory_planner_B.ix_c = b_A->size[0] * b_A->size[1] -
7061 1;
7062 for (cartesian_trajectory_planner_B.kend = 0;
7063 cartesian_trajectory_planner_B.kend <=
7064 cartesian_trajectory_planner_B.ix_c;
7065 cartesian_trajectory_planner_B.kend++) {
7066 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
7067 data[cartesian_trajectory_planner_B.kend];
7068 }
7069
7070 cartesian_trajectory_planner_B.b_h =
7071 (cartesian_trajectory_planner_B.ii +
7072 cartesian_trajectory_planner_B.mmi) + 1;
7073 for (cartesian_trajectory_planner_B.itemp =
7074 cartesian_trajectory_planner_B.pvt;
7075 cartesian_trajectory_planner_B.itemp <=
7076 cartesian_trajectory_planner_B.b_h;
7077 cartesian_trajectory_planner_B.itemp++) {
7078 c_x->data[cartesian_trajectory_planner_B.itemp - 1] *=
7079 cartesian_trajectory_planner_B.smax;
7080 }
7081
7082 cartesian_trajectory_planner_B.smax =
7083 cartesian_trajectory_planner_B.scale_i;
7084 }
7085 }
7086 }
7087
7088 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
7089 b_A->size[0] = c_x->size[0];
7090 b_A->size[1] = c_x->size[1];
7091 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
7092 cartesian_trajectory_planner_B.ix_c = c_x->size[0] * c_x->size[1] - 1;
7093 for (cartesian_trajectory_planner_B.kend = 0;
7094 cartesian_trajectory_planner_B.kend <=
7095 cartesian_trajectory_planner_B.ix_c;
7096 cartesian_trajectory_planner_B.kend++) {
7097 b_A->data[cartesian_trajectory_planner_B.kend] = c_x->
7098 data[cartesian_trajectory_planner_B.kend];
7099 }
7100
7101 b_A->data[cartesian_trajectory_planner_B.ii] =
7102 cartesian_trajectory_planner_B.smax;
7103 } else {
7104 tau->data[cartesian_trajectory_planner_B.m_d1] = 0.0;
7105 }
7106
7107 if (cartesian_trajectory_planner_B.m_d1 + 1 < n) {
7108 cartesian_trajectory_planner_B.smax = b_A->
7109 data[cartesian_trajectory_planner_B.ii];
7110 b_A->data[cartesian_trajectory_planner_B.ii] = 1.0;
7111 cartesian_trajectory_planner_B.pvt = (cartesian_trajectory_planner_B.ii +
7112 cartesian_trajectory_planner_B.ma) + 1;
7113 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
7114 c_x->size[0] = b_A->size[0];
7115 c_x->size[1] = b_A->size[1];
7116 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
7117 cartesian_trajectory_planner_B.ix_c = b_A->size[0] * b_A->size[1] - 1;
7118 for (cartesian_trajectory_planner_B.kend = 0;
7119 cartesian_trajectory_planner_B.kend <=
7120 cartesian_trajectory_planner_B.ix_c;
7121 cartesian_trajectory_planner_B.kend++) {
7122 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
7123 data[cartesian_trajectory_planner_B.kend];
7124 }
7125
7126 if (tau->data[cartesian_trajectory_planner_B.m_d1] != 0.0) {
7127 cartesian_trajectory_planner_B.itemp =
7128 cartesian_trajectory_planner_B.mmi;
7129 cartesian_trajectory_planner_B.kend = cartesian_trajectory_planner_B.ii
7130 + cartesian_trajectory_planner_B.mmi;
7131 while ((cartesian_trajectory_planner_B.itemp + 1 > 0) && (b_A->
7132 data[cartesian_trajectory_planner_B.kend] == 0.0)) {
7133 cartesian_trajectory_planner_B.itemp--;
7134 cartesian_trajectory_planner_B.kend--;
7135 }
7136
7137 cartesian_trajectory_planner_B.nmi--;
7138 exitg2 = false;
7139 while ((!exitg2) && (cartesian_trajectory_planner_B.nmi > 0)) {
7140 cartesian_trajectory_planner_B.ix_c =
7141 (cartesian_trajectory_planner_B.nmi - 1) *
7142 cartesian_trajectory_planner_B.ma +
7143 cartesian_trajectory_planner_B.pvt;
7144 cartesian_trajectory_planner_B.kend =
7145 cartesian_trajectory_planner_B.ix_c;
7146 do {
7147 exitg1 = 0;
7148 if (cartesian_trajectory_planner_B.kend <=
7149 cartesian_trajectory_planner_B.ix_c +
7150 cartesian_trajectory_planner_B.itemp) {
7151 if (b_A->data[cartesian_trajectory_planner_B.kend - 1] != 0.0) {
7152 exitg1 = 1;
7153 } else {
7154 cartesian_trajectory_planner_B.kend++;
7155 }
7156 } else {
7157 cartesian_trajectory_planner_B.nmi--;
7158 exitg1 = 2;
7159 }
7160 } while (exitg1 == 0);
7161
7162 if (exitg1 == 1) {
7163 exitg2 = true;
7164 }
7165 }
7166
7167 cartesian_trajectory_planner_B.lastc =
7168 cartesian_trajectory_planner_B.nmi - 1;
7169 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
7170 c_x->size[0] = b_A->size[0];
7171 c_x->size[1] = b_A->size[1];
7172 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
7173 cartesian_trajectory_planner_B.ix_c = b_A->size[0] * b_A->size[1] - 1;
7174 for (cartesian_trajectory_planner_B.kend = 0;
7175 cartesian_trajectory_planner_B.kend <=
7176 cartesian_trajectory_planner_B.ix_c;
7177 cartesian_trajectory_planner_B.kend++) {
7178 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
7179 data[cartesian_trajectory_planner_B.kend];
7180 }
7181 } else {
7182 cartesian_trajectory_planner_B.itemp = -1;
7183 cartesian_trajectory_planner_B.lastc = -1;
7184 }
7185
7186 if (cartesian_trajectory_planner_B.itemp + 1 > 0) {
7187 if (cartesian_trajectory_planner_B.lastc + 1 != 0) {
7188 for (cartesian_trajectory_planner_B.kend = 0;
7189 cartesian_trajectory_planner_B.kend <=
7190 cartesian_trajectory_planner_B.lastc;
7191 cartesian_trajectory_planner_B.kend++) {
7192 work->data[cartesian_trajectory_planner_B.kend] = 0.0;
7193 }
7194
7195 cartesian_trajectory_planner_B.iy_j = 0;
7196 cartesian_trajectory_planner_B.b_h = cartesian_trajectory_planner_B.ma
7197 * cartesian_trajectory_planner_B.lastc +
7198 cartesian_trajectory_planner_B.pvt;
7199 for (cartesian_trajectory_planner_B.nmi =
7200 cartesian_trajectory_planner_B.pvt;
7201 cartesian_trajectory_planner_B.ma < 0 ?
7202 cartesian_trajectory_planner_B.nmi >=
7203 cartesian_trajectory_planner_B.b_h :
7204 cartesian_trajectory_planner_B.nmi <=
7205 cartesian_trajectory_planner_B.b_h;
7206 cartesian_trajectory_planner_B.nmi +=
7207 cartesian_trajectory_planner_B.ma) {
7208 cartesian_trajectory_planner_B.ix_c =
7209 cartesian_trajectory_planner_B.ii;
7210 cartesian_trajectory_planner_B.scale_i = 0.0;
7211 cartesian_trajectory_planner_B.d_k =
7212 cartesian_trajectory_planner_B.nmi +
7213 cartesian_trajectory_planner_B.itemp;
7214 for (cartesian_trajectory_planner_B.kend =
7215 cartesian_trajectory_planner_B.nmi;
7216 cartesian_trajectory_planner_B.kend <=
7217 cartesian_trajectory_planner_B.d_k;
7218 cartesian_trajectory_planner_B.kend++) {
7219 cartesian_trajectory_planner_B.scale_i += c_x->
7220 data[cartesian_trajectory_planner_B.kend - 1] * c_x->
7221 data[cartesian_trajectory_planner_B.ix_c];
7222 cartesian_trajectory_planner_B.ix_c++;
7223 }
7224
7225 work->data[cartesian_trajectory_planner_B.iy_j] +=
7226 cartesian_trajectory_planner_B.scale_i;
7227 cartesian_trajectory_planner_B.iy_j++;
7228 }
7229 }
7230
7231 if (!(-tau->data[cartesian_trajectory_planner_B.m_d1] == 0.0)) {
7232 cartesian_trajectory_planner_B.iy_j = 0;
7233 for (cartesian_trajectory_planner_B.kend = 0;
7234 cartesian_trajectory_planner_B.kend <=
7235 cartesian_trajectory_planner_B.lastc;
7236 cartesian_trajectory_planner_B.kend++) {
7237 if (work->data[cartesian_trajectory_planner_B.iy_j] != 0.0) {
7238 cartesian_trajectory_planner_B.scale_i = work->
7239 data[cartesian_trajectory_planner_B.iy_j] * -tau->
7240 data[cartesian_trajectory_planner_B.m_d1];
7241 cartesian_trajectory_planner_B.ix_c =
7242 cartesian_trajectory_planner_B.ii;
7243 cartesian_trajectory_planner_B.b_h =
7244 cartesian_trajectory_planner_B.itemp +
7245 cartesian_trajectory_planner_B.pvt;
7246 for (cartesian_trajectory_planner_B.nmi =
7247 cartesian_trajectory_planner_B.pvt;
7248 cartesian_trajectory_planner_B.nmi <=
7249 cartesian_trajectory_planner_B.b_h;
7250 cartesian_trajectory_planner_B.nmi++) {
7251 c_x->data[cartesian_trajectory_planner_B.nmi - 1] += c_x->
7252 data[cartesian_trajectory_planner_B.ix_c] *
7253 cartesian_trajectory_planner_B.scale_i;
7254 cartesian_trajectory_planner_B.ix_c++;
7255 }
7256 }
7257
7258 cartesian_trajectory_planner_B.iy_j++;
7259 cartesian_trajectory_planner_B.pvt +=
7260 cartesian_trajectory_planner_B.ma;
7261 }
7262 }
7263 }
7264
7265 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
7266 b_A->size[0] = c_x->size[0];
7267 b_A->size[1] = c_x->size[1];
7268 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
7269 cartesian_trajectory_planner_B.ix_c = c_x->size[0] * c_x->size[1] - 1;
7270 for (cartesian_trajectory_planner_B.kend = 0;
7271 cartesian_trajectory_planner_B.kend <=
7272 cartesian_trajectory_planner_B.ix_c;
7273 cartesian_trajectory_planner_B.kend++) {
7274 b_A->data[cartesian_trajectory_planner_B.kend] = c_x->
7275 data[cartesian_trajectory_planner_B.kend];
7276 }
7277
7278 b_A->data[cartesian_trajectory_planner_B.ii] =
7279 cartesian_trajectory_planner_B.smax;
7280 }
7281
7282 for (cartesian_trajectory_planner_B.ii = cartesian_trajectory_planner_B.m_d1
7283 + 2; cartesian_trajectory_planner_B.ii <= n;
7284 cartesian_trajectory_planner_B.ii++) {
7285 cartesian_trajectory_planner_B.pvt = ((cartesian_trajectory_planner_B.ii -
7286 1) * cartesian_trajectory_planner_B.ma +
7287 cartesian_trajectory_planner_B.m_d1) + 1;
7288 if (vn1->data[cartesian_trajectory_planner_B.ii - 1] != 0.0) {
7289 cartesian_trajectory_planner_B.smax = fabs(b_A->
7290 data[cartesian_trajectory_planner_B.pvt - 1]) / vn1->
7291 data[cartesian_trajectory_planner_B.ii - 1];
7292 cartesian_trajectory_planner_B.smax = 1.0 -
7293 cartesian_trajectory_planner_B.smax *
7294 cartesian_trajectory_planner_B.smax;
7295 if (cartesian_trajectory_planner_B.smax < 0.0) {
7296 cartesian_trajectory_planner_B.smax = 0.0;
7297 }
7298
7299 cartesian_trajectory_planner_B.scale_i = vn1->
7300 data[cartesian_trajectory_planner_B.ii - 1] / vn2->
7301 data[cartesian_trajectory_planner_B.ii - 1];
7302 cartesian_trajectory_planner_B.scale_i =
7303 cartesian_trajectory_planner_B.scale_i *
7304 cartesian_trajectory_planner_B.scale_i *
7305 cartesian_trajectory_planner_B.smax;
7306 if (cartesian_trajectory_planner_B.scale_i <= 1.4901161193847656E-8) {
7307 if (cartesian_trajectory_planner_B.m_d1 + 1 < m) {
7308 cartesian_trajectory_planner_B.smax = 0.0;
7309 if (cartesian_trajectory_planner_B.mmi >= 1) {
7310 if (cartesian_trajectory_planner_B.mmi == 1) {
7311 cartesian_trajectory_planner_B.smax = fabs(b_A->
7312 data[cartesian_trajectory_planner_B.pvt]);
7313 } else {
7314 cartesian_trajectory_planner_B.scale_i = 3.3121686421112381E-170;
7315 cartesian_trajectory_planner_B.kend =
7316 cartesian_trajectory_planner_B.pvt +
7317 cartesian_trajectory_planner_B.mmi;
7318 for (cartesian_trajectory_planner_B.itemp =
7319 cartesian_trajectory_planner_B.pvt + 1;
7320 cartesian_trajectory_planner_B.itemp <=
7321 cartesian_trajectory_planner_B.kend;
7322 cartesian_trajectory_planner_B.itemp++) {
7323 cartesian_trajectory_planner_B.absxk = fabs(b_A->
7324 data[cartesian_trajectory_planner_B.itemp - 1]);
7325 if (cartesian_trajectory_planner_B.absxk >
7326 cartesian_trajectory_planner_B.scale_i) {
7327 cartesian_trajectory_planner_B.t =
7328 cartesian_trajectory_planner_B.scale_i /
7329 cartesian_trajectory_planner_B.absxk;
7330 cartesian_trajectory_planner_B.smax =
7331 cartesian_trajectory_planner_B.smax *
7332 cartesian_trajectory_planner_B.t *
7333 cartesian_trajectory_planner_B.t + 1.0;
7334 cartesian_trajectory_planner_B.scale_i =
7335 cartesian_trajectory_planner_B.absxk;
7336 } else {
7337 cartesian_trajectory_planner_B.t =
7338 cartesian_trajectory_planner_B.absxk /
7339 cartesian_trajectory_planner_B.scale_i;
7340 cartesian_trajectory_planner_B.smax +=
7341 cartesian_trajectory_planner_B.t *
7342 cartesian_trajectory_planner_B.t;
7343 }
7344 }
7345
7346 cartesian_trajectory_planner_B.smax =
7347 cartesian_trajectory_planner_B.scale_i * sqrt
7348 (cartesian_trajectory_planner_B.smax);
7349 }
7350 }
7351
7352 vn1->data[cartesian_trajectory_planner_B.ii - 1] =
7353 cartesian_trajectory_planner_B.smax;
7354 vn2->data[cartesian_trajectory_planner_B.ii - 1] = vn1->
7355 data[cartesian_trajectory_planner_B.ii - 1];
7356 } else {
7357 vn1->data[cartesian_trajectory_planner_B.ii - 1] = 0.0;
7358 vn2->data[cartesian_trajectory_planner_B.ii - 1] = 0.0;
7359 }
7360 } else {
7361 vn1->data[cartesian_trajectory_planner_B.ii - 1] *= sqrt
7362 (cartesian_trajectory_planner_B.smax);
7363 }
7364 }
7365 }
7366 }
7367
7368 cartesian_trajec_emxFree_real_T(&c_x);
7369 cartesian_trajec_emxFree_real_T(&vn2);
7370 cartesian_trajec_emxFree_real_T(&vn1);
7371 cartesian_trajec_emxFree_real_T(&work);
7372}
7373
7374static void cartesian_trajectory_pl_xzgetrf(int32_T m, int32_T n, const
7375 emxArray_real_T_cartesian_tra_T *A, int32_T lda,
7376 emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T *ipiv,
7377 int32_T *info)
7378{
7379 int32_T mmj;
7380 int32_T b;
7381 int32_T c;
7382 emxArray_real_T_cartesian_tra_T *c_x;
7383 int32_T ix;
7384 real_T smax;
7385 real_T s;
7386 int32_T iy;
7387 int32_T n_0;
7388 int32_T yk;
7389 int32_T k;
7390 int32_T jA;
7391 int32_T jy;
7392 int32_T c_0;
7393 int32_T c_tmp;
7394 k = b_A->size[0] * b_A->size[1];
7395 b_A->size[0] = A->size[0];
7396 b_A->size[1] = A->size[1];
7397 cartes_emxEnsureCapacity_real_T(b_A, k);
7398 iy = A->size[0] * A->size[1] - 1;
7399 for (k = 0; k <= iy; k++) {
7400 b_A->data[k] = A->data[k];
7401 }
7402
7403 if (m < n) {
7404 n_0 = m;
7405 } else {
7406 n_0 = n;
7407 }
7408
7409 if (n_0 < 1) {
7410 n_0 = 0;
7411 }
7412
7413 k = ipiv->size[0] * ipiv->size[1];
7414 ipiv->size[0] = 1;
7415 ipiv->size[1] = n_0;
7416 carte_emxEnsureCapacity_int32_T(ipiv, k);
7417 if (n_0 > 0) {
7418 ipiv->data[0] = 1;
7419 yk = 1;
7420 for (k = 2; k <= n_0; k++) {
7421 yk++;
7422 ipiv->data[k - 1] = yk;
7423 }
7424 }
7425
7426 yk = 0;
7427 cartesian_trajec_emxInit_real_T(&c_x, 2);
7428 if ((m < 1) || (n < 1)) {
7429 } else {
7430 n_0 = m - 1;
7431 if (n_0 >= n) {
7432 n_0 = n;
7433 }
7434
7435 b = n_0 - 1;
7436 for (n_0 = 0; n_0 <= b; n_0++) {
7437 mmj = m - n_0;
7438 c_tmp = (lda + 1) * n_0;
7439 c = c_tmp + 2;
7440 if (mmj < 1) {
7441 iy = 0;
7442 } else {
7443 iy = 1;
7444 if (mmj > 1) {
7445 ix = c - 2;
7446 smax = fabs(b_A->data[c_tmp]);
7447 for (k = 2; k <= mmj; k++) {
7448 ix++;
7449 s = fabs(b_A->data[ix]);
7450 if (s > smax) {
7451 iy = k;
7452 smax = s;
7453 }
7454 }
7455 }
7456 }
7457
7458 if (b_A->data[(c + iy) - 3] != 0.0) {
7459 if (iy - 1 != 0) {
7460 k = n_0 + iy;
7461 ipiv->data[n_0] = k;
7462 ix = c_x->size[0] * c_x->size[1];
7463 c_x->size[0] = b_A->size[0];
7464 c_x->size[1] = b_A->size[1];
7465 cartes_emxEnsureCapacity_real_T(c_x, ix);
7466 iy = b_A->size[0] * b_A->size[1] - 1;
7467 for (ix = 0; ix <= iy; ix++) {
7468 c_x->data[ix] = b_A->data[ix];
7469 }
7470
7471 ix = n_0;
7472 iy = k - 1;
7473 for (k = 0; k < n; k++) {
7474 smax = c_x->data[ix];
7475 c_x->data[ix] = c_x->data[iy];
7476 c_x->data[iy] = smax;
7477 ix += lda;
7478 iy += lda;
7479 }
7480
7481 k = b_A->size[0] * b_A->size[1];
7482 b_A->size[0] = c_x->size[0];
7483 b_A->size[1] = c_x->size[1];
7484 cartes_emxEnsureCapacity_real_T(b_A, k);
7485 iy = c_x->size[0] * c_x->size[1] - 1;
7486 for (k = 0; k <= iy; k++) {
7487 b_A->data[k] = c_x->data[k];
7488 }
7489 }
7490
7491 iy = c + mmj;
7492 for (k = c; k <= iy - 2; k++) {
7493 b_A->data[k - 1] /= b_A->data[c_tmp];
7494 }
7495 } else {
7496 yk = n_0 + 1;
7497 }
7498
7499 iy = (n - n_0) - 2;
7500 jy = c_tmp + lda;
7501 jA = jy + 1;
7502 for (k = 0; k <= iy; k++) {
7503 smax = b_A->data[jy];
7504 if (b_A->data[jy] != 0.0) {
7505 ix = c - 1;
7506 c_0 = (mmj + jA) - 1;
7507 for (c_tmp = jA + 1; c_tmp <= c_0; c_tmp++) {
7508 b_A->data[c_tmp - 1] += b_A->data[ix] * -smax;
7509 ix++;
7510 }
7511 }
7512
7513 jy += lda;
7514 jA += lda;
7515 }
7516 }
7517
7518 if ((yk == 0) && (m <= n) && (!(b_A->data[((m - 1) * b_A->size[0] + m) - 1]
7519 != 0.0))) {
7520 yk = m;
7521 }
7522 }
7523
7524 cartesian_trajec_emxFree_real_T(&c_x);
7525 *info = yk;
7526}
7527
7528static void cartesian_trajectory_plan_xtrsm(int32_T m, int32_T n, const
7529 emxArray_real_T_cartesian_tra_T *A, int32_T lda, const
7530 emxArray_real_T_cartesian_tra_T *B, int32_T ldb,
7531 emxArray_real_T_cartesian_tra_T *b_B)
7532{
7533 int32_T jBcol;
7534 int32_T kAcol;
7535 int32_T i;
7536 int32_T k;
7537 int32_T b;
7538 int32_T b_i;
7539 int32_T loop_ub;
7540 int32_T i_0;
7541 int32_T tmp;
7542 i_0 = b_B->size[0] * b_B->size[1];
7543 b_B->size[0] = B->size[0];
7544 b_B->size[1] = B->size[1];
7545 cartes_emxEnsureCapacity_real_T(b_B, i_0);
7546 loop_ub = B->size[0] * B->size[1] - 1;
7547 for (i_0 = 0; i_0 <= loop_ub; i_0++) {
7548 b_B->data[i_0] = B->data[i_0];
7549 }
7550
7551 if ((n == 0) || ((B->size[0] == 0) || (B->size[1] == 0))) {
7552 } else {
7553 for (loop_ub = 0; loop_ub < n; loop_ub++) {
7554 jBcol = ldb * loop_ub - 1;
7555 for (k = m; k >= 1; k--) {
7556 kAcol = (k - 1) * lda - 1;
7557 i_0 = k + jBcol;
7558 if (b_B->data[i_0] != 0.0) {
7559 b_B->data[i_0] /= A->data[k + kAcol];
7560 b = k - 2;
7561 for (b_i = 0; b_i <= b; b_i++) {
7562 i = b_i + 1;
7563 tmp = i + jBcol;
7564 b_B->data[tmp] -= b_B->data[i_0] * A->data[i + kAcol];
7565 }
7566 }
7567 }
7568 }
7569 }
7570}
7571
7572static void cartesian_traje_emxFree_int32_T(emxArray_int32_T_cartesian_tr_T
7573 **pEmxArray)
7574{
7575 if (*pEmxArray != (emxArray_int32_T_cartesian_tr_T *)NULL) {
7576 if (((*pEmxArray)->data != (int32_T *)NULL) && (*pEmxArray)->canFreeData) {
7577 free((*pEmxArray)->data);
7578 }
7579
7580 free((*pEmxArray)->size);
7581 free(*pEmxArray);
7582 *pEmxArray = (emxArray_int32_T_cartesian_tr_T *)NULL;
7583 }
7584}
7585
7586static void cartesian_trajectory_p_mldivide(const
7587 emxArray_real_T_cartesian_tra_T *A, const emxArray_real_T_cartesian_tra_T *B,
7588 emxArray_real_T_cartesian_tra_T *Y)
7589{
7590 emxArray_real_T_cartesian_tra_T *c_A;
7591 emxArray_real_T_cartesian_tra_T *b_tau;
7592 emxArray_int32_T_cartesian_tr_T *b_jpvt;
7593 emxArray_real_T_cartesian_tra_T *B_0;
7594 emxArray_int32_T_cartesian_tr_T *b_jpvt_0;
7595 boolean_T guard1 = false;
7596 cartesian_trajec_emxInit_real_T(&c_A, 2);
7597 cartesian_trajec_emxInit_real_T(&b_tau, 1);
7598 cartesian_traje_emxInit_int32_T(&b_jpvt, 2);
7599 cartesian_trajec_emxInit_real_T(&B_0, 2);
7600 cartesian_traje_emxInit_int32_T(&b_jpvt_0, 2);
7601 if ((A->size[0] == 0) || (A->size[1] == 0) || ((B->size[0] == 0) || (B->size[1]
7602 == 0))) {
7603 cartesian_trajectory_planner_B.minmn = A->size[1];
7604 cartesian_trajectory_planner_B.minmana = B->size[1];
7605 cartesian_trajectory_planner_B.b_i_a = Y->size[0] * Y->size[1];
7606 Y->size[0] = cartesian_trajectory_planner_B.minmn;
7607 Y->size[1] = cartesian_trajectory_planner_B.minmana;
7608 cartes_emxEnsureCapacity_real_T(Y, cartesian_trajectory_planner_B.b_i_a);
7609 cartesian_trajectory_planner_B.minmn = cartesian_trajectory_planner_B.minmn *
7610 cartesian_trajectory_planner_B.minmana - 1;
7611 for (cartesian_trajectory_planner_B.b_i_a = 0;
7612 cartesian_trajectory_planner_B.b_i_a <=
7613 cartesian_trajectory_planner_B.minmn;
7614 cartesian_trajectory_planner_B.b_i_a++) {
7615 Y->data[cartesian_trajectory_planner_B.b_i_a] = 0.0;
7616 }
7617 } else if (A->size[0] == A->size[1]) {
7618 cartesian_trajectory_planner_B.minmn = A->size[0];
7619 cartesian_trajectory_planner_B.rankR = A->size[1];
7620 if (cartesian_trajectory_planner_B.minmn <
7621 cartesian_trajectory_planner_B.rankR) {
7622 cartesian_trajectory_planner_B.rankR =
7623 cartesian_trajectory_planner_B.minmn;
7624 }
7625
7626 cartesian_trajectory_planner_B.minmn = B->size[0];
7627 if (cartesian_trajectory_planner_B.minmn <
7628 cartesian_trajectory_planner_B.rankR) {
7629 cartesian_trajectory_planner_B.rankR =
7630 cartesian_trajectory_planner_B.minmn;
7631 }
7632
7633 cartesian_trajectory_planner_B.nb = B->size[1] - 1;
7634 cartesian_trajectory_pl_xzgetrf(cartesian_trajectory_planner_B.rankR,
7635 cartesian_trajectory_planner_B.rankR, A, A->size[0], c_A, b_jpvt,
7636 &cartesian_trajectory_planner_B.minmn);
7637 cartesian_trajectory_planner_B.b_i_a = B_0->size[0] * B_0->size[1];
7638 B_0->size[0] = B->size[0];
7639 B_0->size[1] = B->size[1];
7640 cartes_emxEnsureCapacity_real_T(B_0, cartesian_trajectory_planner_B.b_i_a);
7641 cartesian_trajectory_planner_B.minmn = B->size[0] * B->size[1] - 1;
7642 for (cartesian_trajectory_planner_B.b_i_a = 0;
7643 cartesian_trajectory_planner_B.b_i_a <=
7644 cartesian_trajectory_planner_B.minmn;
7645 cartesian_trajectory_planner_B.b_i_a++) {
7646 B_0->data[cartesian_trajectory_planner_B.b_i_a] = B->
7647 data[cartesian_trajectory_planner_B.b_i_a];
7648 }
7649
7650 cartesian_trajectory_planner_B.minmn = cartesian_trajectory_planner_B.rankR
7651 - 2;
7652 for (cartesian_trajectory_planner_B.b_i_a = 0;
7653 cartesian_trajectory_planner_B.b_i_a <=
7654 cartesian_trajectory_planner_B.minmn;
7655 cartesian_trajectory_planner_B.b_i_a++) {
7656 if (cartesian_trajectory_planner_B.b_i_a + 1 != b_jpvt->
7657 data[cartesian_trajectory_planner_B.b_i_a]) {
7658 cartesian_trajectory_planner_B.na = b_jpvt->
7659 data[cartesian_trajectory_planner_B.b_i_a] - 1;
7660 for (cartesian_trajectory_planner_B.minmana = 0;
7661 cartesian_trajectory_planner_B.minmana <=
7662 cartesian_trajectory_planner_B.nb;
7663 cartesian_trajectory_planner_B.minmana++) {
7664 cartesian_trajectory_planner_B.tol_l = B_0->data[B_0->size[0] *
7665 cartesian_trajectory_planner_B.minmana +
7666 cartesian_trajectory_planner_B.b_i_a];
7667 B_0->data[cartesian_trajectory_planner_B.b_i_a + B_0->size[0] *
7668 cartesian_trajectory_planner_B.minmana] = B_0->data[B_0->size[0] *
7669 cartesian_trajectory_planner_B.minmana +
7670 cartesian_trajectory_planner_B.na];
7671 B_0->data[cartesian_trajectory_planner_B.na + B_0->size[0] *
7672 cartesian_trajectory_planner_B.minmana] =
7673 cartesian_trajectory_planner_B.tol_l;
7674 }
7675 }
7676 }
7677
7678 if ((B->size[1] == 0) || ((B_0->size[0] == 0) || (B_0->size[1] == 0))) {
7679 } else {
7680 for (cartesian_trajectory_planner_B.minmana = 0;
7681 cartesian_trajectory_planner_B.minmana <=
7682 cartesian_trajectory_planner_B.nb;
7683 cartesian_trajectory_planner_B.minmana++) {
7684 cartesian_trajectory_planner_B.m_m = B->size[0] *
7685 cartesian_trajectory_planner_B.minmana - 1;
7686 for (cartesian_trajectory_planner_B.minmn = 0;
7687 cartesian_trajectory_planner_B.minmn <
7688 cartesian_trajectory_planner_B.rankR;
7689 cartesian_trajectory_planner_B.minmn++) {
7690 cartesian_trajectory_planner_B.nb_k = c_A->size[0] *
7691 cartesian_trajectory_planner_B.minmn - 1;
7692 cartesian_trajectory_planner_B.b_i_a =
7693 (cartesian_trajectory_planner_B.minmn +
7694 cartesian_trajectory_planner_B.m_m) + 1;
7695 if (B_0->data[cartesian_trajectory_planner_B.b_i_a] != 0.0) {
7696 for (cartesian_trajectory_planner_B.na =
7697 cartesian_trajectory_planner_B.minmn + 2;
7698 cartesian_trajectory_planner_B.na <=
7699 cartesian_trajectory_planner_B.rankR;
7700 cartesian_trajectory_planner_B.na++) {
7701 cartesian_trajectory_planner_B.mn =
7702 cartesian_trajectory_planner_B.na +
7703 cartesian_trajectory_planner_B.m_m;
7704 B_0->data[cartesian_trajectory_planner_B.mn] -= B_0->
7705 data[cartesian_trajectory_planner_B.b_i_a] * c_A->
7706 data[cartesian_trajectory_planner_B.na +
7707 cartesian_trajectory_planner_B.nb_k];
7708 }
7709 }
7710 }
7711 }
7712 }
7713
7714 cartesian_trajectory_plan_xtrsm(cartesian_trajectory_planner_B.rankR,
7715 B->size[1], c_A, c_A->size[0], B_0, B->size[0], Y);
7716 } else {
7717 cartesian_trajectory_planner_B.na = A->size[1] - 1;
7718 cartesian_trajectory_planner_B.b_i_a = c_A->size[0] * c_A->size[1];
7719 c_A->size[0] = A->size[0];
7720 c_A->size[1] = A->size[1];
7721 cartes_emxEnsureCapacity_real_T(c_A, cartesian_trajectory_planner_B.b_i_a);
7722 cartesian_trajectory_planner_B.minmn = A->size[0] * A->size[1] - 1;
7723 for (cartesian_trajectory_planner_B.b_i_a = 0;
7724 cartesian_trajectory_planner_B.b_i_a <=
7725 cartesian_trajectory_planner_B.minmn;
7726 cartesian_trajectory_planner_B.b_i_a++) {
7727 c_A->data[cartesian_trajectory_planner_B.b_i_a] = A->
7728 data[cartesian_trajectory_planner_B.b_i_a];
7729 }
7730
7731 cartesian_trajectory_planner_B.minmn = A->size[0];
7732 cartesian_trajectory_planner_B.minmana = A->size[1];
7733 if (cartesian_trajectory_planner_B.minmn <
7734 cartesian_trajectory_planner_B.minmana) {
7735 cartesian_trajectory_planner_B.minmana =
7736 cartesian_trajectory_planner_B.minmn;
7737 }
7738
7739 cartesian_trajectory_planner_B.b_i_a = b_tau->size[0];
7740 b_tau->size[0] = cartesian_trajectory_planner_B.minmana;
7741 cartes_emxEnsureCapacity_real_T(b_tau, cartesian_trajectory_planner_B.b_i_a);
7742 for (cartesian_trajectory_planner_B.b_i_a = 0;
7743 cartesian_trajectory_planner_B.b_i_a <
7744 cartesian_trajectory_planner_B.minmana;
7745 cartesian_trajectory_planner_B.b_i_a++) {
7746 b_tau->data[cartesian_trajectory_planner_B.b_i_a] = 0.0;
7747 }
7748
7749 guard1 = false;
7750 if ((A->size[0] == 0) || (A->size[1] == 0)) {
7751 guard1 = true;
7752 } else {
7753 cartesian_trajectory_planner_B.minmn = A->size[0];
7754 cartesian_trajectory_planner_B.minmana = A->size[1];
7755 if (cartesian_trajectory_planner_B.minmn <
7756 cartesian_trajectory_planner_B.minmana) {
7757 cartesian_trajectory_planner_B.minmana =
7758 cartesian_trajectory_planner_B.minmn;
7759 }
7760
7761 if (cartesian_trajectory_planner_B.minmana < 1) {
7762 guard1 = true;
7763 } else {
7764 cartesian_trajectory_planner_B.b_i_a = b_jpvt->size[0] * b_jpvt->size[1];
7765 b_jpvt->size[0] = 1;
7766 b_jpvt->size[1] = A->size[1];
7767 carte_emxEnsureCapacity_int32_T(b_jpvt,
7768 cartesian_trajectory_planner_B.b_i_a);
7769 cartesian_trajectory_planner_B.minmn = A->size[1] - 1;
7770 for (cartesian_trajectory_planner_B.b_i_a = 0;
7771 cartesian_trajectory_planner_B.b_i_a <=
7772 cartesian_trajectory_planner_B.minmn;
7773 cartesian_trajectory_planner_B.b_i_a++) {
7774 b_jpvt->data[cartesian_trajectory_planner_B.b_i_a] = 0;
7775 }
7776
7777 for (cartesian_trajectory_planner_B.minmn = 0;
7778 cartesian_trajectory_planner_B.minmn <=
7779 cartesian_trajectory_planner_B.na;
7780 cartesian_trajectory_planner_B.minmn++) {
7781 b_jpvt->data[cartesian_trajectory_planner_B.minmn] =
7782 cartesian_trajectory_planner_B.minmn + 1;
7783 }
7784
7785 cartesian_trajectory_planner_B.b_i_a = b_jpvt_0->size[0] *
7786 b_jpvt_0->size[1];
7787 b_jpvt_0->size[0] = 1;
7788 b_jpvt_0->size[1] = b_jpvt->size[1];
7789 carte_emxEnsureCapacity_int32_T(b_jpvt_0,
7790 cartesian_trajectory_planner_B.b_i_a);
7791 cartesian_trajectory_planner_B.minmn = b_jpvt->size[0] * b_jpvt->size[1];
7792 for (cartesian_trajectory_planner_B.b_i_a = 0;
7793 cartesian_trajectory_planner_B.b_i_a <
7794 cartesian_trajectory_planner_B.minmn;
7795 cartesian_trajectory_planner_B.b_i_a++) {
7796 b_jpvt_0->data[cartesian_trajectory_planner_B.b_i_a] = b_jpvt->
7797 data[cartesian_trajectory_planner_B.b_i_a];
7798 }
7799
7800 cartesian_trajectory_pla_qrpf_a(A, A->size[0], A->size[1], b_tau,
7801 b_jpvt_0, c_A, b_jpvt);
7802 }
7803 }
7804
7805 if (guard1) {
7806 cartesian_trajectory_planner_B.b_i_a = b_jpvt->size[0] * b_jpvt->size[1];
7807 b_jpvt->size[0] = 1;
7808 b_jpvt->size[1] = A->size[1];
7809 carte_emxEnsureCapacity_int32_T(b_jpvt,
7810 cartesian_trajectory_planner_B.b_i_a);
7811 cartesian_trajectory_planner_B.minmn = A->size[1] - 1;
7812 for (cartesian_trajectory_planner_B.b_i_a = 0;
7813 cartesian_trajectory_planner_B.b_i_a <=
7814 cartesian_trajectory_planner_B.minmn;
7815 cartesian_trajectory_planner_B.b_i_a++) {
7816 b_jpvt->data[cartesian_trajectory_planner_B.b_i_a] = 0;
7817 }
7818
7819 for (cartesian_trajectory_planner_B.minmana = 0;
7820 cartesian_trajectory_planner_B.minmana <=
7821 cartesian_trajectory_planner_B.na;
7822 cartesian_trajectory_planner_B.minmana++) {
7823 b_jpvt->data[cartesian_trajectory_planner_B.minmana] =
7824 cartesian_trajectory_planner_B.minmana + 1;
7825 }
7826 }
7827
7828 cartesian_trajectory_planner_B.rankR = 0;
7829 if (c_A->size[0] < c_A->size[1]) {
7830 cartesian_trajectory_planner_B.minmn = c_A->size[0];
7831 cartesian_trajectory_planner_B.minmana = c_A->size[1];
7832 } else {
7833 cartesian_trajectory_planner_B.minmn = c_A->size[1];
7834 cartesian_trajectory_planner_B.minmana = c_A->size[0];
7835 }
7836
7837 if (cartesian_trajectory_planner_B.minmn > 0) {
7838 cartesian_trajectory_planner_B.tol_l = 2.2204460492503131E-15 *
7839 static_cast<real_T>(cartesian_trajectory_planner_B.minmana);
7840 if (1.4901161193847656E-8 < cartesian_trajectory_planner_B.tol_l) {
7841 cartesian_trajectory_planner_B.tol_l = 1.4901161193847656E-8;
7842 }
7843
7844 cartesian_trajectory_planner_B.tol_l *= fabs(c_A->data[0]);
7845 while ((cartesian_trajectory_planner_B.rankR <
7846 cartesian_trajectory_planner_B.minmn) && (!(fabs(c_A->data
7847 [c_A->size[0] * cartesian_trajectory_planner_B.rankR +
7848 cartesian_trajectory_planner_B.rankR]) <=
7849 cartesian_trajectory_planner_B.tol_l))) {
7850 cartesian_trajectory_planner_B.rankR++;
7851 }
7852 }
7853
7854 cartesian_trajectory_planner_B.nb = B->size[1] - 1;
7855 cartesian_trajectory_planner_B.minmn = c_A->size[1];
7856 cartesian_trajectory_planner_B.minmana = B->size[1];
7857 cartesian_trajectory_planner_B.b_i_a = Y->size[0] * Y->size[1];
7858 Y->size[0] = cartesian_trajectory_planner_B.minmn;
7859 Y->size[1] = cartesian_trajectory_planner_B.minmana;
7860 cartes_emxEnsureCapacity_real_T(Y, cartesian_trajectory_planner_B.b_i_a);
7861 cartesian_trajectory_planner_B.minmn = cartesian_trajectory_planner_B.minmn *
7862 cartesian_trajectory_planner_B.minmana - 1;
7863 for (cartesian_trajectory_planner_B.b_i_a = 0;
7864 cartesian_trajectory_planner_B.b_i_a <=
7865 cartesian_trajectory_planner_B.minmn;
7866 cartesian_trajectory_planner_B.b_i_a++) {
7867 Y->data[cartesian_trajectory_planner_B.b_i_a] = 0.0;
7868 }
7869
7870 cartesian_trajectory_planner_B.b_i_a = B_0->size[0] * B_0->size[1];
7871 B_0->size[0] = B->size[0];
7872 B_0->size[1] = B->size[1];
7873 cartes_emxEnsureCapacity_real_T(B_0, cartesian_trajectory_planner_B.b_i_a);
7874 cartesian_trajectory_planner_B.minmn = B->size[0] * B->size[1] - 1;
7875 for (cartesian_trajectory_planner_B.b_i_a = 0;
7876 cartesian_trajectory_planner_B.b_i_a <=
7877 cartesian_trajectory_planner_B.minmn;
7878 cartesian_trajectory_planner_B.b_i_a++) {
7879 B_0->data[cartesian_trajectory_planner_B.b_i_a] = B->
7880 data[cartesian_trajectory_planner_B.b_i_a];
7881 }
7882
7883 cartesian_trajectory_planner_B.m_m = c_A->size[0];
7884 cartesian_trajectory_planner_B.nb_k = B->size[1] - 1;
7885 cartesian_trajectory_planner_B.minmn = c_A->size[0];
7886 cartesian_trajectory_planner_B.minmana = c_A->size[1];
7887 if (cartesian_trajectory_planner_B.minmn <
7888 cartesian_trajectory_planner_B.minmana) {
7889 cartesian_trajectory_planner_B.minmana =
7890 cartesian_trajectory_planner_B.minmn;
7891 }
7892
7893 cartesian_trajectory_planner_B.mn = cartesian_trajectory_planner_B.minmana -
7894 1;
7895 for (cartesian_trajectory_planner_B.minmana = 0;
7896 cartesian_trajectory_planner_B.minmana <=
7897 cartesian_trajectory_planner_B.mn;
7898 cartesian_trajectory_planner_B.minmana++) {
7899 if (b_tau->data[cartesian_trajectory_planner_B.minmana] != 0.0) {
7900 for (cartesian_trajectory_planner_B.minmn = 0;
7901 cartesian_trajectory_planner_B.minmn <=
7902 cartesian_trajectory_planner_B.nb_k;
7903 cartesian_trajectory_planner_B.minmn++) {
7904 cartesian_trajectory_planner_B.tol_l = B_0->data[B_0->size[0] *
7905 cartesian_trajectory_planner_B.minmn +
7906 cartesian_trajectory_planner_B.minmana];
7907 for (cartesian_trajectory_planner_B.na =
7908 cartesian_trajectory_planner_B.minmana + 2;
7909 cartesian_trajectory_planner_B.na <=
7910 cartesian_trajectory_planner_B.m_m;
7911 cartesian_trajectory_planner_B.na++) {
7912 cartesian_trajectory_planner_B.tol_l += c_A->data[(c_A->size[0] *
7913 cartesian_trajectory_planner_B.minmana +
7914 cartesian_trajectory_planner_B.na) - 1] * B_0->data[(B_0->size[0] *
7915 cartesian_trajectory_planner_B.minmn +
7916 cartesian_trajectory_planner_B.na) - 1];
7917 }
7918
7919 cartesian_trajectory_planner_B.tol_l *= b_tau->
7920 data[cartesian_trajectory_planner_B.minmana];
7921 if (cartesian_trajectory_planner_B.tol_l != 0.0) {
7922 B_0->data[cartesian_trajectory_planner_B.minmana + B_0->size[0] *
7923 cartesian_trajectory_planner_B.minmn] -=
7924 cartesian_trajectory_planner_B.tol_l;
7925 for (cartesian_trajectory_planner_B.b_i_a =
7926 cartesian_trajectory_planner_B.minmana + 2;
7927 cartesian_trajectory_planner_B.b_i_a <=
7928 cartesian_trajectory_planner_B.m_m;
7929 cartesian_trajectory_planner_B.b_i_a++) {
7930 B_0->data[(cartesian_trajectory_planner_B.b_i_a + B_0->size[0] *
7931 cartesian_trajectory_planner_B.minmn) - 1] -= c_A->
7932 data[(c_A->size[0] * cartesian_trajectory_planner_B.minmana +
7933 cartesian_trajectory_planner_B.b_i_a) - 1] *
7934 cartesian_trajectory_planner_B.tol_l;
7935 }
7936 }
7937 }
7938 }
7939 }
7940
7941 for (cartesian_trajectory_planner_B.minmn = 0;
7942 cartesian_trajectory_planner_B.minmn <=
7943 cartesian_trajectory_planner_B.nb; cartesian_trajectory_planner_B.minmn
7944 ++) {
7945 for (cartesian_trajectory_planner_B.b_i_a = 0;
7946 cartesian_trajectory_planner_B.b_i_a <
7947 cartesian_trajectory_planner_B.rankR;
7948 cartesian_trajectory_planner_B.b_i_a++) {
7949 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.b_i_a] + Y->size[0]
7950 * cartesian_trajectory_planner_B.minmn) - 1] = B_0->data
7951 [B_0->size[0] * cartesian_trajectory_planner_B.minmn +
7952 cartesian_trajectory_planner_B.b_i_a];
7953 }
7954
7955 for (cartesian_trajectory_planner_B.minmana =
7956 cartesian_trajectory_planner_B.rankR;
7957 cartesian_trajectory_planner_B.minmana >= 1;
7958 cartesian_trajectory_planner_B.minmana--) {
7959 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.minmana - 1] +
7960 Y->size[0] * cartesian_trajectory_planner_B.minmn) - 1] /=
7961 c_A->data[((cartesian_trajectory_planner_B.minmana - 1) * c_A->size[0]
7962 + cartesian_trajectory_planner_B.minmana) - 1];
7963 cartesian_trajectory_planner_B.na =
7964 cartesian_trajectory_planner_B.minmana - 2;
7965 for (cartesian_trajectory_planner_B.b_i_a = 0;
7966 cartesian_trajectory_planner_B.b_i_a <=
7967 cartesian_trajectory_planner_B.na;
7968 cartesian_trajectory_planner_B.b_i_a++) {
7969 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.b_i_a] + Y->size
7970 [0] * cartesian_trajectory_planner_B.minmn) - 1] -= Y->data
7971 [(b_jpvt->data[cartesian_trajectory_planner_B.minmana - 1] + Y->
7972 size[0] * cartesian_trajectory_planner_B.minmn) - 1] * c_A->data
7973 [(cartesian_trajectory_planner_B.minmana - 1) * c_A->size[0] +
7974 cartesian_trajectory_planner_B.b_i_a];
7975 }
7976 }
7977 }
7978 }
7979
7980 cartesian_traje_emxFree_int32_T(&b_jpvt_0);
7981 cartesian_trajec_emxFree_real_T(&B_0);
7982 cartesian_traje_emxFree_int32_T(&b_jpvt);
7983 cartesian_trajec_emxFree_real_T(&b_tau);
7984 cartesian_trajec_emxFree_real_T(&c_A);
7985}
7986
7987static void cartesian_tra_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
7988 **pEmxArray)
7989{
7990 if (*pEmxArray != (emxArray_boolean_T_cartesian__T *)NULL) {
7991 if (((*pEmxArray)->data != (boolean_T *)NULL) && (*pEmxArray)->canFreeData)
7992 {
7993 free((*pEmxArray)->data);
7994 }
7995
7996 free((*pEmxArray)->size);
7997 free(*pEmxArray);
7998 *pEmxArray = (emxArray_boolean_T_cartesian__T *)NULL;
7999 }
8000}
8001
8002static boolean_T DampedBFGSwGradientProjection_a(const
8003 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
8004 emxArray_real_T_cartesian_tra_T *alpha)
8005{
8006 boolean_T flag;
8007 boolean_T y;
8008 emxArray_boolean_T_cartesian__T *x;
8009 int32_T ix;
8010 int32_T loop_ub;
8011 boolean_T exitg1;
8012 cartesian_tra_emxInit_boolean_T(&x, 1);
8013 if (cartesian_trajectory_pla_norm_a(Hg) < obj->GradientTolerance) {
8014 ix = x->size[0];
8015 x->size[0] = alpha->size[0];
8016 car_emxEnsureCapacity_boolean_T(x, ix);
8017 loop_ub = alpha->size[0];
8018 for (ix = 0; ix < loop_ub; ix++) {
8019 x->data[ix] = (alpha->data[ix] <= 0.0);
8020 }
8021
8022 y = true;
8023 ix = 0;
8024 exitg1 = false;
8025 while ((!exitg1) && (ix + 1 <= x->size[0])) {
8026 if (!x->data[ix]) {
8027 y = false;
8028 exitg1 = true;
8029 } else {
8030 ix++;
8031 }
8032 }
8033
8034 if (y) {
8035 flag = true;
8036 } else {
8037 flag = false;
8038 }
8039 } else {
8040 flag = false;
8041 }
8042
8043 cartesian_tra_emxFree_boolean_T(&x);
8044 return flag;
8045}
8046
8047static void cartesian_trajectory_planne_inv(const
8048 emxArray_real_T_cartesian_tra_T *x, emxArray_real_T_cartesian_tra_T *y)
8049{
8050 int32_T n;
8051 emxArray_int32_T_cartesian_tr_T *p;
8052 int32_T c;
8053 emxArray_real_T_cartesian_tra_T *c_A;
8054 emxArray_int32_T_cartesian_tr_T *b_ipiv;
8055 int32_T info;
8056 int32_T n_0;
8057 int32_T yk;
8058 emxArray_real_T_cartesian_tra_T *y_0;
8059 if ((x->size[0] == 0) || (x->size[1] == 0)) {
8060 info = y->size[0] * y->size[1];
8061 y->size[0] = x->size[0];
8062 y->size[1] = x->size[1];
8063 cartes_emxEnsureCapacity_real_T(y, info);
8064 n_0 = x->size[0] * x->size[1] - 1;
8065 for (info = 0; info <= n_0; info++) {
8066 y->data[info] = x->data[info];
8067 }
8068 } else {
8069 n = x->size[0];
8070 info = y->size[0] * y->size[1];
8071 y->size[0] = x->size[0];
8072 y->size[1] = x->size[1];
8073 cartes_emxEnsureCapacity_real_T(y, info);
8074 n_0 = x->size[0] * x->size[1] - 1;
8075 for (info = 0; info <= n_0; info++) {
8076 y->data[info] = 0.0;
8077 }
8078
8079 cartesian_traje_emxInit_int32_T(&p, 2);
8080 cartesian_trajec_emxInit_real_T(&c_A, 2);
8081 cartesian_traje_emxInit_int32_T(&b_ipiv, 2);
8082 cartesian_trajectory_pl_xzgetrf(x->size[0], x->size[0], x, x->size[0], c_A,
8083 b_ipiv, &info);
8084 if (x->size[0] < 1) {
8085 n_0 = 0;
8086 } else {
8087 n_0 = x->size[0];
8088 }
8089
8090 info = p->size[0] * p->size[1];
8091 p->size[0] = 1;
8092 p->size[1] = n_0;
8093 carte_emxEnsureCapacity_int32_T(p, info);
8094 if (n_0 > 0) {
8095 p->data[0] = 1;
8096 yk = 1;
8097 for (info = 2; info <= n_0; info++) {
8098 yk++;
8099 p->data[info - 1] = yk;
8100 }
8101 }
8102
8103 n_0 = b_ipiv->size[1] - 1;
8104 for (info = 0; info <= n_0; info++) {
8105 if (b_ipiv->data[info] > static_cast<real_T>(info) + 1.0) {
8106 yk = p->data[b_ipiv->data[info] - 1];
8107 p->data[b_ipiv->data[info] - 1] = p->data[info];
8108 p->data[info] = yk;
8109 }
8110 }
8111
8112 cartesian_traje_emxFree_int32_T(&b_ipiv);
8113 for (info = 0; info < n; info++) {
8114 c = p->data[info] - 1;
8115 y->data[info + y->size[0] * (p->data[info] - 1)] = 1.0;
8116 for (n_0 = info + 1; n_0 <= n; n_0++) {
8117 if (y->data[(y->size[0] * c + n_0) - 1] != 0.0) {
8118 for (yk = n_0 + 1; yk <= n; yk++) {
8119 y->data[(yk + y->size[0] * c) - 1] -= c_A->data[((n_0 - 1) *
8120 c_A->size[0] + yk) - 1] * y->data[(y->size[0] * c + n_0) - 1];
8121 }
8122 }
8123 }
8124 }
8125
8126 cartesian_traje_emxFree_int32_T(&p);
8127 cartesian_trajec_emxInit_real_T(&y_0, 2);
8128 info = y_0->size[0] * y_0->size[1];
8129 y_0->size[0] = y->size[0];
8130 y_0->size[1] = y->size[1];
8131 cartes_emxEnsureCapacity_real_T(y_0, info);
8132 n_0 = y->size[0] * y->size[1];
8133 for (info = 0; info < n_0; info++) {
8134 y_0->data[info] = y->data[info];
8135 }
8136
8137 cartesian_trajectory_plan_xtrsm(x->size[0], x->size[0], c_A, x->size[0], y_0,
8138 x->size[0], y);
8139 cartesian_trajec_emxFree_real_T(&y_0);
8140 cartesian_trajec_emxFree_real_T(&c_A);
8141 }
8142}
8143
8144static void cartesian_trajectory_plann_diag(const
8145 emxArray_real_T_cartesian_tra_T *v, emxArray_real_T_cartesian_tra_T *d)
8146{
8147 int32_T u0;
8148 int32_T u1;
8149 if ((v->size[0] == 1) && (v->size[1] == 1)) {
8150 u0 = d->size[0];
8151 d->size[0] = 1;
8152 cartes_emxEnsureCapacity_real_T(d, u0);
8153 d->data[0] = v->data[0];
8154 } else {
8155 if (0 < v->size[1]) {
8156 u0 = v->size[0];
8157 u1 = v->size[1];
8158 if (u0 < u1) {
8159 u1 = u0;
8160 }
8161 } else {
8162 u1 = 0;
8163 }
8164
8165 u0 = d->size[0];
8166 d->size[0] = u1;
8167 cartes_emxEnsureCapacity_real_T(d, u0);
8168 for (u0 = 0; u0 < u1; u0++) {
8169 d->data[u0] = v->data[v->size[0] * u0 + u0];
8170 }
8171 }
8172}
8173
8174static void cartesian_trajectory_pl_sqrt_as(emxArray_real_T_cartesian_tra_T *x)
8175{
8176 int32_T nx;
8177 int32_T b_k;
8178 nx = x->size[0] - 1;
8179 for (b_k = 0; b_k <= nx; b_k++) {
8180 x->data[b_k] = sqrt(x->data[b_k]);
8181 }
8182}
8183
8184static boolean_T cartesian_trajectory_planne_any(const
8185 emxArray_boolean_T_cartesian__T *x)
8186{
8187 boolean_T y;
8188 int32_T ix;
8189 boolean_T exitg1;
8190 y = false;
8191 ix = 0;
8192 exitg1 = false;
8193 while ((!exitg1) && (ix + 1 <= x->size[0])) {
8194 if (!x->data[ix]) {
8195 ix++;
8196 } else {
8197 y = true;
8198 exitg1 = true;
8199 }
8200 }
8201
8202 return y;
8203}
8204
8205static boolean_T cartesian_tr_isPositiveDefinite(const real_T B[36])
8206{
8207 emxArray_real_T_cartesian_tra_T *b_x;
8208 boolean_T exitg1;
8209 cartesian_trajectory_planner_B.c_A_size_idx_0 = 6;
8210 cartesian_trajectory_planner_B.c_A_size_idx_1 = 6;
8211 memcpy(&cartesian_trajectory_planner_B.c_A_data[0], &B[0], 36U * sizeof(real_T));
8212 cartesian_trajectory_planner_B.b_info = 0;
8213 cartesian_trajectory_planner_B.b_j_kt = 1;
8214 cartesian_trajec_emxInit_real_T(&b_x, 2);
8215 exitg1 = false;
8216 while ((!exitg1) && (cartesian_trajectory_planner_B.b_j_kt - 1 < 6)) {
8217 cartesian_trajectory_planner_B.jm1 = cartesian_trajectory_planner_B.b_j_kt -
8218 2;
8219 cartesian_trajectory_planner_B.idxAjj =
8220 ((cartesian_trajectory_planner_B.b_j_kt - 1) * 6 +
8221 cartesian_trajectory_planner_B.b_j_kt) - 1;
8222 cartesian_trajectory_planner_B.ssq = 0.0;
8223 if (cartesian_trajectory_planner_B.b_j_kt - 1 >= 1) {
8224 cartesian_trajectory_planner_B.ix_k =
8225 cartesian_trajectory_planner_B.b_j_kt - 1;
8226 cartesian_trajectory_planner_B.iy = cartesian_trajectory_planner_B.b_j_kt
8227 - 1;
8228 for (cartesian_trajectory_planner_B.k_p = 0;
8229 cartesian_trajectory_planner_B.k_p <=
8230 cartesian_trajectory_planner_B.jm1;
8231 cartesian_trajectory_planner_B.k_p++) {
8232 cartesian_trajectory_planner_B.ssq +=
8233 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_k]
8234 * cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.iy];
8235 cartesian_trajectory_planner_B.ix_k += 6;
8236 cartesian_trajectory_planner_B.iy += 6;
8237 }
8238 }
8239
8240 cartesian_trajectory_planner_B.ssq =
8241 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.idxAjj]
8242 - cartesian_trajectory_planner_B.ssq;
8243 if (cartesian_trajectory_planner_B.ssq > 0.0) {
8244 cartesian_trajectory_planner_B.ssq = sqrt
8245 (cartesian_trajectory_planner_B.ssq);
8246 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.idxAjj]
8247 = cartesian_trajectory_planner_B.ssq;
8248 if (cartesian_trajectory_planner_B.b_j_kt < 6) {
8249 if (cartesian_trajectory_planner_B.b_j_kt - 1 != 0) {
8250 cartesian_trajectory_planner_B.ix_k =
8251 cartesian_trajectory_planner_B.b_j_kt - 1;
8252 cartesian_trajectory_planner_B.jm1 =
8253 (cartesian_trajectory_planner_B.b_j_kt - 2) * 6 +
8254 cartesian_trajectory_planner_B.b_j_kt;
8255 for (cartesian_trajectory_planner_B.k_p =
8256 cartesian_trajectory_planner_B.b_j_kt + 1;
8257 cartesian_trajectory_planner_B.k_p <=
8258 cartesian_trajectory_planner_B.jm1 + 1;
8259 cartesian_trajectory_planner_B.k_p += 6) {
8260 cartesian_trajectory_planner_B.c =
8261 -cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_k];
8262 cartesian_trajectory_planner_B.iy =
8263 cartesian_trajectory_planner_B.idxAjj + 1;
8264 cartesian_trajectory_planner_B.d_ds =
8265 cartesian_trajectory_planner_B.k_p -
8266 cartesian_trajectory_planner_B.b_j_kt;
8267 for (cartesian_trajectory_planner_B.ia =
8268 cartesian_trajectory_planner_B.k_p;
8269 cartesian_trajectory_planner_B.ia <=
8270 cartesian_trajectory_planner_B.d_ds + 5;
8271 cartesian_trajectory_planner_B.ia++) {
8272 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.iy]
8273 +=
8274 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ia
8275 - 1] * cartesian_trajectory_planner_B.c;
8276 cartesian_trajectory_planner_B.iy++;
8277 }
8278
8279 cartesian_trajectory_planner_B.ix_k += 6;
8280 }
8281 }
8282
8283 cartesian_trajectory_planner_B.ssq = 1.0 /
8284 cartesian_trajectory_planner_B.ssq;
8285 cartesian_trajectory_planner_B.ix_k = b_x->size[0] * b_x->size[1];
8286 b_x->size[0] = 6;
8287 b_x->size[1] = 6;
8288 cartes_emxEnsureCapacity_real_T(b_x, cartesian_trajectory_planner_B.ix_k);
8289 cartesian_trajectory_planner_B.c_A_size_idx_0 =
8290 cartesian_trajectory_planner_B.c_A_size_idx_0 *
8291 cartesian_trajectory_planner_B.c_A_size_idx_1 - 1;
8292 for (cartesian_trajectory_planner_B.ix_k = 0;
8293 cartesian_trajectory_planner_B.ix_k <=
8294 cartesian_trajectory_planner_B.c_A_size_idx_0;
8295 cartesian_trajectory_planner_B.ix_k++) {
8296 b_x->data[cartesian_trajectory_planner_B.ix_k] =
8297 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_k];
8298 }
8299
8300 cartesian_trajectory_planner_B.jm1 =
8301 cartesian_trajectory_planner_B.idxAjj -
8302 cartesian_trajectory_planner_B.b_j_kt;
8303 for (cartesian_trajectory_planner_B.k_p =
8304 cartesian_trajectory_planner_B.idxAjj + 2;
8305 cartesian_trajectory_planner_B.k_p <=
8306 cartesian_trajectory_planner_B.jm1 + 7;
8307 cartesian_trajectory_planner_B.k_p++) {
8308 b_x->data[cartesian_trajectory_planner_B.k_p - 1] *=
8309 cartesian_trajectory_planner_B.ssq;
8310 }
8311
8312 cartesian_trajectory_planner_B.c_A_size_idx_0 = b_x->size[0];
8313 cartesian_trajectory_planner_B.c_A_size_idx_1 = b_x->size[1];
8314 for (cartesian_trajectory_planner_B.ix_k = 0;
8315 cartesian_trajectory_planner_B.ix_k < 36;
8316 cartesian_trajectory_planner_B.ix_k++) {
8317 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_k]
8318 = b_x->data[cartesian_trajectory_planner_B.ix_k];
8319 }
8320 }
8321
8322 cartesian_trajectory_planner_B.b_j_kt++;
8323 } else {
8324 cartesian_trajectory_planner_B.b_info =
8325 cartesian_trajectory_planner_B.b_j_kt;
8326 exitg1 = true;
8327 }
8328 }
8329
8330 cartesian_trajec_emxFree_real_T(&b_x);
8331 return cartesian_trajectory_planner_B.b_info == 0;
8332}
8333
8334static boolean_T DampedBFGSwGradientProjectio_as(const
8335 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6])
8336{
8337 boolean_T flag;
8338 emxArray_real_T_cartesian_tra_T *b;
8339 emxArray_real_T_cartesian_tra_T *c;
8340 int32_T m;
8341 int32_T inner;
8342 int32_T b_i;
8343 int32_T loop_ub;
8344 emxArray_boolean_T_cartesian__T *c_0;
8345 cartesian_trajec_emxInit_real_T(&b, 2);
8346 cartesian_trajec_emxInit_real_T(&c, 1);
8347 cartesian_tra_emxInit_boolean_T(&c_0, 1);
8348 if (obj->ConstraintsOn) {
8349 b_i = b->size[0] * b->size[1];
8350 b->size[0] = obj->ConstraintMatrix->size[0];
8351 b->size[1] = obj->ConstraintMatrix->size[1];
8352 cartes_emxEnsureCapacity_real_T(b, b_i);
8353 loop_ub = obj->ConstraintMatrix->size[0] * obj->ConstraintMatrix->size[1] -
8354 1;
8355 for (b_i = 0; b_i <= loop_ub; b_i++) {
8356 b->data[b_i] = obj->ConstraintMatrix->data[b_i];
8357 }
8358
8359 m = b->size[1] - 1;
8360 inner = b->size[0] - 1;
8361 b_i = c->size[0];
8362 c->size[0] = b->size[1];
8363 cartes_emxEnsureCapacity_real_T(c, b_i);
8364 for (b_i = 0; b_i <= m; b_i++) {
8365 c->data[b_i] = 0.0;
8366 }
8367
8368 for (b_i = 0; b_i <= inner; b_i++) {
8369 for (loop_ub = 0; loop_ub <= m; loop_ub++) {
8370 c->data[loop_ub] += b->data[loop_ub * b->size[0] + b_i] * xNew[b_i];
8371 }
8372 }
8373
8374 b_i = c_0->size[0];
8375 c_0->size[0] = c->size[0];
8376 car_emxEnsureCapacity_boolean_T(c_0, b_i);
8377 loop_ub = c->size[0];
8378 for (b_i = 0; b_i < loop_ub; b_i++) {
8379 c_0->data[b_i] = (c->data[b_i] - obj->ConstraintBound->data[b_i] >
8380 1.4901161193847656E-8);
8381 }
8382
8383 if (cartesian_trajectory_planne_any(c_0)) {
8384 flag = true;
8385 } else {
8386 flag = false;
8387 }
8388 } else {
8389 flag = false;
8390 }
8391
8392 cartesian_tra_emxFree_boolean_T(&c_0);
8393 cartesian_trajec_emxFree_real_T(&c);
8394 cartesian_trajec_emxFree_real_T(&b);
8395 return flag;
8396}
8397
8398static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
8399 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
8400 *iter)
8401{
8402 emxArray_real_T_cartesian_tra_T *unusedU1;
8403 emxArray_real_T_cartesian_tra_T *grad;
8404 emxArray_boolean_T_cartesian__T *activeSet;
8405 emxArray_real_T_cartesian_tra_T *A;
8406 emxArray_real_T_cartesian_tra_T *alpha;
8407 emxArray_real_T_cartesian_tra_T *AIn;
8408 emxArray_real_T_cartesian_tra_T *L;
8409 f_robotics_manip_internal_IKE_T *b;
8410 f_robotics_manip_internal_IKE_T *c;
8411 f_robotics_manip_internal_IKE_T *d;
8412 emxArray_int32_T_cartesian_tr_T *cb;
8413 emxArray_int32_T_cartesian_tr_T *db;
8414 emxArray_int32_T_cartesian_tr_T *eb;
8415 emxArray_int32_T_cartesian_tr_T *fb;
8416 emxArray_int32_T_cartesian_tr_T *gb;
8417 f_robotics_manip_internal_IKE_T *args;
8418 emxArray_real_T_cartesian_tra_T *a;
8419 emxArray_int32_T_cartesian_tr_T *ii;
8420 emxArray_real_T_cartesian_tra_T *y;
8421 emxArray_int32_T_cartesian_tr_T *ii_0;
8422 emxArray_real_T_cartesian_tra_T *y_0;
8423 emxArray_boolean_T_cartesian__T *x;
8424 emxArray_real_T_cartesian_tra_T *A_0;
8425 emxArray_real_T_cartesian_tra_T *A_1;
8426 emxArray_real_T_cartesian_tra_T *A_2;
8427 emxArray_real_T_cartesian_tra_T *sigma;
8428 emxArray_real_T_cartesian_tra_T *tmp;
8429 emxArray_real_T_cartesian_tra_T *tmp_0;
8430 emxArray_real_T_cartesian_tra_T *grad_0;
8431 emxArray_real_T_cartesian_tra_T *sNew;
8432 emxArray_int32_T_cartesian_tr_T *ii_1;
8433 emxArray_int32_T_cartesian_tr_T *ii_2;
8434 emxArray_real_T_cartesian_tra_T *grad_1;
8435 emxArray_real_T_cartesian_tra_T *A_3;
8436 emxArray_real_T_cartesian_tra_T *alpha_0;
8437 emxArray_int32_T_cartesian_tr_T *ii_3;
8438 static const int8_T tmp_1[36] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1,
8439 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };
8440
8441 int32_T exitg1;
8442 int32_T exitg2;
8443 boolean_T exitg3;
8444 boolean_T guard1 = false;
8445 boolean_T guard2 = false;
8446 for (cartesian_trajectory_planner_B.i_h = 0;
8447 cartesian_trajectory_planner_B.i_h < 6;
8448 cartesian_trajectory_planner_B.i_h++) {
8449 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_h] =
8450 obj->SeedInternal[cartesian_trajectory_planner_B.i_h];
8451 }
8452
8453 cartesian_trajec_emxInit_real_T(&unusedU1, 2);
8454 cartesian_trajec_emxInit_real_T(&grad, 1);
8455 obj->TimeObjInternal.StartTime = ctimefun();
8456 cartesian_IKHelpers_computeCost(cartesian_trajectory_planner_B.x,
8457 obj->ExtraArgs, &cartesian_trajectory_planner_B.cost,
8458 cartesian_trajectory_planner_B.unusedU0, unusedU1, &b);
8459 obj->ExtraArgs = b;
8460 args = obj->ExtraArgs;
8461 cartesian_trajectory_planner_B.b_i = grad->size[0];
8462 grad->size[0] = args->GradTemp->size[0];
8463 cartes_emxEnsureCapacity_real_T(grad, cartesian_trajectory_planner_B.b_i);
8464 cartesian_trajectory_planner_B.i_h = args->GradTemp->size[0];
8465 for (cartesian_trajectory_planner_B.b_i = 0;
8466 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.i_h;
8467 cartesian_trajectory_planner_B.b_i++) {
8468 grad->data[cartesian_trajectory_planner_B.b_i] = args->GradTemp->
8469 data[cartesian_trajectory_planner_B.b_i];
8470 }
8471
8472 cartesian_trajectory_planne_eye(cartesian_trajectory_planner_B.unusedU0);
8473 memcpy(&cartesian_trajectory_planner_B.H[0],
8474 &cartesian_trajectory_planner_B.unusedU0[0], 36U * sizeof(real_T));
8475 cartesian_tra_emxInit_boolean_T(&activeSet, 1);
8476 cartesian_trajec_emxInit_real_T(&A, 2);
8477 cartesian_trajec_emxInit_real_T(&alpha, 1);
8478 cartesian_traje_emxInit_int32_T(&ii, 1);
8479 if (obj->ConstraintsOn) {
8480 cartesian_trajectory_planner_B.b_i = A->size[0] * A->size[1];
8481 A->size[0] = obj->ConstraintMatrix->size[0];
8482 A->size[1] = obj->ConstraintMatrix->size[1];
8483 cartes_emxEnsureCapacity_real_T(A, cartesian_trajectory_planner_B.b_i);
8484 cartesian_trajectory_planner_B.i_h = obj->ConstraintMatrix->size[0] *
8485 obj->ConstraintMatrix->size[1] - 1;
8486 for (cartesian_trajectory_planner_B.b_i = 0;
8487 cartesian_trajectory_planner_B.b_i <=
8488 cartesian_trajectory_planner_B.i_h; cartesian_trajectory_planner_B.b_i
8489 ++) {
8490 A->data[cartesian_trajectory_planner_B.b_i] = obj->ConstraintMatrix->
8491 data[cartesian_trajectory_planner_B.b_i];
8492 }
8493
8494 cartesian_trajectory_planner_B.m_k = A->size[1] - 1;
8495 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
8496 cartesian_trajectory_planner_B.b_i = alpha->size[0];
8497 alpha->size[0] = A->size[1];
8498 cartes_emxEnsureCapacity_real_T(alpha, cartesian_trajectory_planner_B.b_i);
8499 for (cartesian_trajectory_planner_B.b_i = 0;
8500 cartesian_trajectory_planner_B.b_i <=
8501 cartesian_trajectory_planner_B.m_k; cartesian_trajectory_planner_B.b_i
8502 ++) {
8503 alpha->data[cartesian_trajectory_planner_B.b_i] = 0.0;
8504 }
8505
8506 for (cartesian_trajectory_planner_B.nx_j = 0;
8507 cartesian_trajectory_planner_B.nx_j <=
8508 cartesian_trajectory_planner_B.inner;
8509 cartesian_trajectory_planner_B.nx_j++) {
8510 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
8511 cartesian_trajectory_planner_B.g_idx_0 <=
8512 cartesian_trajectory_planner_B.m_k;
8513 cartesian_trajectory_planner_B.g_idx_0++) {
8514 alpha->data[cartesian_trajectory_planner_B.g_idx_0] += A->
8515 data[cartesian_trajectory_planner_B.g_idx_0 * A->size[0] +
8516 cartesian_trajectory_planner_B.nx_j] *
8517 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.nx_j];
8518 }
8519 }
8520
8521 cartesian_trajectory_planner_B.b_i = activeSet->size[0];
8522 activeSet->size[0] = alpha->size[0];
8523 car_emxEnsureCapacity_boolean_T(activeSet,
8524 cartesian_trajectory_planner_B.b_i);
8525 cartesian_trajectory_planner_B.i_h = alpha->size[0];
8526 for (cartesian_trajectory_planner_B.b_i = 0;
8527 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.i_h;
8528 cartesian_trajectory_planner_B.b_i++) {
8529 activeSet->data[cartesian_trajectory_planner_B.b_i] = (alpha->
8530 data[cartesian_trajectory_planner_B.b_i] >= obj->ConstraintBound->
8531 data[cartesian_trajectory_planner_B.b_i]);
8532 }
8533
8534 cartesian_trajectory_planner_B.nx_j = activeSet->size[0] - 1;
8535 cartesian_trajectory_planner_B.idx = 0;
8536 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
8537 cartesian_trajectory_planner_B.g_idx_0 <=
8538 cartesian_trajectory_planner_B.nx_j;
8539 cartesian_trajectory_planner_B.g_idx_0++) {
8540 if (activeSet->data[cartesian_trajectory_planner_B.g_idx_0]) {
8541 cartesian_trajectory_planner_B.idx++;
8542 }
8543 }
8544
8545 cartesian_trajectory_planner_B.b_i = ii->size[0];
8546 ii->size[0] = cartesian_trajectory_planner_B.idx;
8547 carte_emxEnsureCapacity_int32_T(ii, cartesian_trajectory_planner_B.b_i);
8548 cartesian_trajectory_planner_B.b_i = 0;
8549 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
8550 cartesian_trajectory_planner_B.g_idx_0 <=
8551 cartesian_trajectory_planner_B.nx_j;
8552 cartesian_trajectory_planner_B.g_idx_0++) {
8553 if (activeSet->data[cartesian_trajectory_planner_B.g_idx_0]) {
8554 ii->data[cartesian_trajectory_planner_B.b_i] =
8555 cartesian_trajectory_planner_B.g_idx_0 + 1;
8556 cartesian_trajectory_planner_B.b_i++;
8557 }
8558 }
8559
8560 cartesian_trajectory_planner_B.i_h = obj->ConstraintMatrix->size[0];
8561 cartesian_trajectory_planner_B.b_i = A->size[0] * A->size[1];
8562 A->size[0] = cartesian_trajectory_planner_B.i_h;
8563 A->size[1] = ii->size[0];
8564 cartes_emxEnsureCapacity_real_T(A, cartesian_trajectory_planner_B.b_i);
8565 cartesian_trajectory_planner_B.n_b = ii->size[0];
8566 for (cartesian_trajectory_planner_B.b_i = 0;
8567 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.n_b;
8568 cartesian_trajectory_planner_B.b_i++) {
8569 for (cartesian_trajectory_planner_B.idx = 0;
8570 cartesian_trajectory_planner_B.idx <
8571 cartesian_trajectory_planner_B.i_h;
8572 cartesian_trajectory_planner_B.idx++) {
8573 A->data[cartesian_trajectory_planner_B.idx + A->size[0] *
8574 cartesian_trajectory_planner_B.b_i] = obj->ConstraintMatrix->data
8575 [(ii->data[cartesian_trajectory_planner_B.b_i] - 1) *
8576 obj->ConstraintMatrix->size[0] + cartesian_trajectory_planner_B.idx];
8577 }
8578 }
8579 } else {
8580 cartesian_trajectory_planner_B.g_idx_0 = obj->ConstraintBound->size[0];
8581 cartesian_trajectory_planner_B.b_i = activeSet->size[0];
8582 activeSet->size[0] = cartesian_trajectory_planner_B.g_idx_0;
8583 car_emxEnsureCapacity_boolean_T(activeSet,
8584 cartesian_trajectory_planner_B.b_i);
8585 for (cartesian_trajectory_planner_B.b_i = 0;
8586 cartesian_trajectory_planner_B.b_i <
8587 cartesian_trajectory_planner_B.g_idx_0;
8588 cartesian_trajectory_planner_B.b_i++) {
8589 activeSet->data[cartesian_trajectory_planner_B.b_i] = false;
8590 }
8591
8592 A->size[0] = 6;
8593 A->size[1] = 0;
8594 }
8595
8596 cartesian_trajectory_planner_B.j = A->size[1] - 1;
8597 cartesian_trajec_emxInit_real_T(&AIn, 2);
8598 cartesian_trajec_emxInit_real_T(&A_0, 2);
8599 cartesian_trajec_emxInit_real_T(&A_1, 1);
8600 for (cartesian_trajectory_planner_B.nx_j = 0;
8601 cartesian_trajectory_planner_B.nx_j <= cartesian_trajectory_planner_B.j;
8602 cartesian_trajectory_planner_B.nx_j++) {
8603 cartesian_trajectory_planner_B.i_h = A->size[0];
8604 cartesian_trajectory_planner_B.b_i = A_0->size[0] * A_0->size[1];
8605 A_0->size[0] = 1;
8606 A_0->size[1] = cartesian_trajectory_planner_B.i_h;
8607 cartes_emxEnsureCapacity_real_T(A_0, cartesian_trajectory_planner_B.b_i);
8608 for (cartesian_trajectory_planner_B.b_i = 0;
8609 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.i_h;
8610 cartesian_trajectory_planner_B.b_i++) {
8611 A_0->data[cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
8612 cartesian_trajectory_planner_B.nx_j + cartesian_trajectory_planner_B.b_i];
8613 }
8614
8615 cartesian_trajectory_planner_B.i_h = A->size[0];
8616 cartesian_trajectory_planner_B.b_i = A_1->size[0];
8617 A_1->size[0] = cartesian_trajectory_planner_B.i_h;
8618 cartes_emxEnsureCapacity_real_T(A_1, cartesian_trajectory_planner_B.b_i);
8619 for (cartesian_trajectory_planner_B.b_i = 0;
8620 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.i_h;
8621 cartesian_trajectory_planner_B.b_i++) {
8622 A_1->data[cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
8623 cartesian_trajectory_planner_B.nx_j + cartesian_trajectory_planner_B.b_i];
8624 }
8625
8626 cartesian_trajectory_planner_B.A_d = 0.0;
8627 for (cartesian_trajectory_planner_B.b_i = 0;
8628 cartesian_trajectory_planner_B.b_i < 6;
8629 cartesian_trajectory_planner_B.b_i++) {
8630 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i] =
8631 0.0;
8632 for (cartesian_trajectory_planner_B.idx = 0;
8633 cartesian_trajectory_planner_B.idx < 6;
8634 cartesian_trajectory_planner_B.idx++) {
8635 cartesian_trajectory_planner_B.s_i = cartesian_trajectory_planner_B.H[6 *
8636 cartesian_trajectory_planner_B.b_i +
8637 cartesian_trajectory_planner_B.idx] * A_0->
8638 data[cartesian_trajectory_planner_B.idx] +
8639 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
8640 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i] =
8641 cartesian_trajectory_planner_B.s_i;
8642 }
8643
8644 cartesian_trajectory_planner_B.A_d +=
8645 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i] *
8646 A_1->data[cartesian_trajectory_planner_B.b_i];
8647 }
8648
8649 cartesian_trajectory_planner_B.s_i = 1.0 /
8650 cartesian_trajectory_planner_B.A_d;
8651 for (cartesian_trajectory_planner_B.b_i = 0;
8652 cartesian_trajectory_planner_B.b_i < 36;
8653 cartesian_trajectory_planner_B.b_i++) {
8654 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.b_i] =
8655 cartesian_trajectory_planner_B.s_i *
8656 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.b_i];
8657 }
8658
8659 cartesian_trajectory_planner_B.i_h = A->size[0];
8660 cartesian_trajectory_planner_B.n_b = A->size[0];
8661 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
8662 AIn->size[0] = cartesian_trajectory_planner_B.i_h;
8663 AIn->size[1] = cartesian_trajectory_planner_B.n_b;
8664 cartes_emxEnsureCapacity_real_T(AIn, cartesian_trajectory_planner_B.b_i);
8665 for (cartesian_trajectory_planner_B.b_i = 0;
8666 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.n_b;
8667 cartesian_trajectory_planner_B.b_i++) {
8668 for (cartesian_trajectory_planner_B.idx = 0;
8669 cartesian_trajectory_planner_B.idx <
8670 cartesian_trajectory_planner_B.i_h;
8671 cartesian_trajectory_planner_B.idx++) {
8672 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
8673 cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
8674 cartesian_trajectory_planner_B.nx_j +
8675 cartesian_trajectory_planner_B.idx] * A->data[A->size[0] *
8676 cartesian_trajectory_planner_B.nx_j +
8677 cartesian_trajectory_planner_B.b_i];
8678 }
8679 }
8680
8681 cartesian_trajectory_planner_B.n_b = AIn->size[1] - 1;
8682 cartesian_trajectory_planner_B.b_i = unusedU1->size[0] * unusedU1->size[1];
8683 unusedU1->size[0] = 6;
8684 unusedU1->size[1] = AIn->size[1];
8685 cartes_emxEnsureCapacity_real_T(unusedU1, cartesian_trajectory_planner_B.b_i);
8686 for (cartesian_trajectory_planner_B.idx = 0;
8687 cartesian_trajectory_planner_B.idx <=
8688 cartesian_trajectory_planner_B.n_b; cartesian_trajectory_planner_B.idx
8689 ++) {
8690 cartesian_trajectory_planner_B.coffset =
8691 cartesian_trajectory_planner_B.idx * 6 - 1;
8692 cartesian_trajectory_planner_B.boffset =
8693 cartesian_trajectory_planner_B.idx * AIn->size[0] - 1;
8694 for (cartesian_trajectory_planner_B.b_i = 0;
8695 cartesian_trajectory_planner_B.b_i < 6;
8696 cartesian_trajectory_planner_B.b_i++) {
8697 cartesian_trajectory_planner_B.s_i = 0.0;
8698 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
8699 cartesian_trajectory_planner_B.g_idx_0 < 6;
8700 cartesian_trajectory_planner_B.g_idx_0++) {
8701 cartesian_trajectory_planner_B.s_i +=
8702 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.g_idx_0
8703 * 6 + cartesian_trajectory_planner_B.b_i] * AIn->data
8704 [(cartesian_trajectory_planner_B.boffset +
8705 cartesian_trajectory_planner_B.g_idx_0) + 1];
8706 }
8707
8708 unusedU1->data[(cartesian_trajectory_planner_B.coffset +
8709 cartesian_trajectory_planner_B.b_i) + 1] =
8710 cartesian_trajectory_planner_B.s_i;
8711 }
8712 }
8713
8714 for (cartesian_trajectory_planner_B.b_i = 0;
8715 cartesian_trajectory_planner_B.b_i < 6;
8716 cartesian_trajectory_planner_B.b_i++) {
8717 for (cartesian_trajectory_planner_B.idx = 0;
8718 cartesian_trajectory_planner_B.idx < 6;
8719 cartesian_trajectory_planner_B.idx++) {
8720 cartesian_trajectory_planner_B.s_i = 0.0;
8721 for (cartesian_trajectory_planner_B.i_h = 0;
8722 cartesian_trajectory_planner_B.i_h < 6;
8723 cartesian_trajectory_planner_B.i_h++) {
8724 cartesian_trajectory_planner_B.s_i += unusedU1->data[6 *
8725 cartesian_trajectory_planner_B.i_h +
8726 cartesian_trajectory_planner_B.b_i] *
8727 cartesian_trajectory_planner_B.H[6 *
8728 cartesian_trajectory_planner_B.idx +
8729 cartesian_trajectory_planner_B.i_h];
8730 }
8731
8732 cartesian_trajectory_planner_B.idxl = 6 *
8733 cartesian_trajectory_planner_B.idx +
8734 cartesian_trajectory_planner_B.b_i;
8735 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.idxl] =
8736 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.idxl]
8737 - cartesian_trajectory_planner_B.s_i;
8738 }
8739 }
8740
8741 memcpy(&cartesian_trajectory_planner_B.H[0],
8742 &cartesian_trajectory_planner_B.H_m[0], 36U * sizeof(real_T));
8743 }
8744
8745 cartesian_trajec_emxFree_real_T(&A_1);
8746 cartesian_trajec_emxFree_real_T(&A_0);
8747 for (cartesian_trajectory_planner_B.i_h = 0;
8748 cartesian_trajectory_planner_B.i_h < 6;
8749 cartesian_trajectory_planner_B.i_h++) {
8750 xSol[cartesian_trajectory_planner_B.i_h] =
8751 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_h];
8752 }
8753
8754 cartesian_trajectory_planner_B.A_d = obj->MaxNumIterationInternal;
8755 cartesian_trajectory_planner_B.g_idx_0 = 0;
8756 cartesian_trajec_emxInit_real_T(&L, 1);
8757 cartesian_traje_emxInit_int32_T(&cb, 1);
8758 cartesian_traje_emxInit_int32_T(&db, 1);
8759 cartesian_traje_emxInit_int32_T(&eb, 1);
8760 cartesian_traje_emxInit_int32_T(&fb, 1);
8761 cartesian_traje_emxInit_int32_T(&gb, 1);
8762 cartesian_trajec_emxInit_real_T(&a, 2);
8763 cartesian_trajec_emxInit_real_T(&y, 1);
8764 cartesian_traje_emxInit_int32_T(&ii_0, 1);
8765 cartesian_trajec_emxInit_real_T(&y_0, 2);
8766 cartesian_tra_emxInit_boolean_T(&x, 1);
8767 cartesian_trajec_emxInit_real_T(&A_2, 2);
8768 cartesian_trajec_emxInit_real_T(&sigma, 2);
8769 cartesian_trajec_emxInit_real_T(&tmp, 2);
8770 cartesian_trajec_emxInit_real_T(&tmp_0, 2);
8771 cartesian_trajec_emxInit_real_T(&grad_0, 2);
8772 cartesian_trajec_emxInit_real_T(&sNew, 2);
8773 cartesian_traje_emxInit_int32_T(&ii_1, 1);
8774 cartesian_traje_emxInit_int32_T(&ii_2, 1);
8775 cartesian_trajec_emxInit_real_T(&grad_1, 2);
8776 cartesian_trajec_emxInit_real_T(&A_3, 2);
8777 cartesian_trajec_emxInit_real_T(&alpha_0, 2);
8778 cartesian_traje_emxInit_int32_T(&ii_3, 1);
8779 do {
8780 exitg2 = 0;
8781 if (cartesian_trajectory_planner_B.g_idx_0 <= static_cast<int32_T>
8782 (cartesian_trajectory_planner_B.A_d) - 1) {
8783 cartesian_trajectory_planner_B.s_i = SystemTimeProvider_getElapsedTi
8784 (&obj->TimeObjInternal);
8785 cartesian_trajectory_planner_B.flag = (cartesian_trajectory_planner_B.s_i >
8786 obj->MaxTimeInternal);
8787 if (cartesian_trajectory_planner_B.flag) {
8788 *exitFlag = TimeLimitExceeded;
8789 args = obj->ExtraArgs;
8790 for (cartesian_trajectory_planner_B.b_i = 0;
8791 cartesian_trajectory_planner_B.b_i < 36;
8792 cartesian_trajectory_planner_B.b_i++) {
8793 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
8794 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
8795 }
8796
8797 cartesian_trajectory_planner_B.b_i = grad->size[0];
8798 grad->size[0] = args->ErrTemp->size[0];
8799 cartes_emxEnsureCapacity_real_T(grad, cartesian_trajectory_planner_B.b_i);
8800 cartesian_trajectory_planner_B.i_h = args->ErrTemp->size[0];
8801 for (cartesian_trajectory_planner_B.b_i = 0;
8802 cartesian_trajectory_planner_B.b_i <
8803 cartesian_trajectory_planner_B.i_h;
8804 cartesian_trajectory_planner_B.b_i++) {
8805 grad->data[cartesian_trajectory_planner_B.b_i] = args->ErrTemp->
8806 data[cartesian_trajectory_planner_B.b_i];
8807 }
8808
8809 for (cartesian_trajectory_planner_B.b_i = 0;
8810 cartesian_trajectory_planner_B.b_i < 6;
8811 cartesian_trajectory_planner_B.b_i++) {
8812 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i] =
8813 0.0;
8814 for (cartesian_trajectory_planner_B.idx = 0;
8815 cartesian_trajectory_planner_B.idx < 6;
8816 cartesian_trajectory_planner_B.idx++) {
8817 cartesian_trajectory_planner_B.A_d =
8818 cartesian_trajectory_planner_B.unusedU0[6 *
8819 cartesian_trajectory_planner_B.idx +
8820 cartesian_trajectory_planner_B.b_i] * grad->
8821 data[cartesian_trajectory_planner_B.idx] +
8822 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
8823 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
8824 = cartesian_trajectory_planner_B.A_d;
8825 }
8826 }
8827
8828 *err = cartesian_trajectory_pla_norm_a(cartesian_trajectory_planner_B.x);
8829 *iter = static_cast<real_T>(cartesian_trajectory_planner_B.g_idx_0) +
8830 1.0;
8831 exitg2 = 1;
8832 } else {
8833 if ((A->size[0] == 0) || (A->size[1] == 0)) {
8834 cartesian_trajectory_planner_B.b_i = alpha->size[0];
8835 alpha->size[0] = 1;
8836 cartes_emxEnsureCapacity_real_T(alpha,
8837 cartesian_trajectory_planner_B.b_i);
8838 alpha->data[0] = 0.0;
8839 } else {
8840 cartesian_trajectory_planner_B.m_k = A->size[1] - 1;
8841 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
8842 cartesian_trajectory_planner_B.n_b = A->size[1] - 1;
8843 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
8844 AIn->size[0] = A->size[1];
8845 AIn->size[1] = A->size[1];
8846 cartes_emxEnsureCapacity_real_T(AIn,
8847 cartesian_trajectory_planner_B.b_i);
8848 for (cartesian_trajectory_planner_B.idx = 0;
8849 cartesian_trajectory_planner_B.idx <=
8850 cartesian_trajectory_planner_B.n_b;
8851 cartesian_trajectory_planner_B.idx++) {
8852 cartesian_trajectory_planner_B.coffset =
8853 (cartesian_trajectory_planner_B.m_k + 1) *
8854 cartesian_trajectory_planner_B.idx - 1;
8855 cartesian_trajectory_planner_B.boffset =
8856 cartesian_trajectory_planner_B.idx * A->size[0] - 1;
8857 for (cartesian_trajectory_planner_B.b_i = 0;
8858 cartesian_trajectory_planner_B.b_i <=
8859 cartesian_trajectory_planner_B.m_k;
8860 cartesian_trajectory_planner_B.b_i++) {
8861 AIn->data[(cartesian_trajectory_planner_B.coffset +
8862 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
8863 }
8864
8865 for (cartesian_trajectory_planner_B.nx_j = 0;
8866 cartesian_trajectory_planner_B.nx_j <=
8867 cartesian_trajectory_planner_B.inner;
8868 cartesian_trajectory_planner_B.nx_j++) {
8869 cartesian_trajectory_planner_B.s_i = A->data
8870 [(cartesian_trajectory_planner_B.boffset +
8871 cartesian_trajectory_planner_B.nx_j) + 1];
8872 for (cartesian_trajectory_planner_B.j = 0;
8873 cartesian_trajectory_planner_B.j <=
8874 cartesian_trajectory_planner_B.m_k;
8875 cartesian_trajectory_planner_B.j++) {
8876 cartesian_trajectory_planner_B.b_i =
8877 (cartesian_trajectory_planner_B.coffset +
8878 cartesian_trajectory_planner_B.j) + 1;
8879 AIn->data[cartesian_trajectory_planner_B.b_i] += A->
8880 data[cartesian_trajectory_planner_B.j * A->size[0] +
8881 cartesian_trajectory_planner_B.nx_j] *
8882 cartesian_trajectory_planner_B.s_i;
8883 }
8884 }
8885 }
8886
8887 cartesian_trajectory_planner_B.b_i = A_2->size[0] * A_2->size[1];
8888 A_2->size[0] = A->size[1];
8889 A_2->size[1] = A->size[0];
8890 cartes_emxEnsureCapacity_real_T(A_2,
8891 cartesian_trajectory_planner_B.b_i);
8892 cartesian_trajectory_planner_B.i_h = A->size[0];
8893 for (cartesian_trajectory_planner_B.b_i = 0;
8894 cartesian_trajectory_planner_B.b_i <
8895 cartesian_trajectory_planner_B.i_h;
8896 cartesian_trajectory_planner_B.b_i++) {
8897 cartesian_trajectory_planner_B.n_b = A->size[1];
8898 for (cartesian_trajectory_planner_B.idx = 0;
8899 cartesian_trajectory_planner_B.idx <
8900 cartesian_trajectory_planner_B.n_b;
8901 cartesian_trajectory_planner_B.idx++) {
8902 A_2->data[cartesian_trajectory_planner_B.idx + A_2->size[0] *
8903 cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
8904 cartesian_trajectory_planner_B.idx +
8905 cartesian_trajectory_planner_B.b_i];
8906 }
8907 }
8908
8909 cartesian_trajectory_p_mldivide(AIn, A_2, a);
8910 cartesian_trajectory_planner_B.m_k = a->size[0] - 1;
8911 cartesian_trajectory_planner_B.inner = a->size[1] - 1;
8912 cartesian_trajectory_planner_B.b_i = alpha->size[0];
8913 alpha->size[0] = a->size[0];
8914 cartes_emxEnsureCapacity_real_T(alpha,
8915 cartesian_trajectory_planner_B.b_i);
8916 for (cartesian_trajectory_planner_B.b_i = 0;
8917 cartesian_trajectory_planner_B.b_i <=
8918 cartesian_trajectory_planner_B.m_k;
8919 cartesian_trajectory_planner_B.b_i++) {
8920 alpha->data[cartesian_trajectory_planner_B.b_i] = 0.0;
8921 }
8922
8923 for (cartesian_trajectory_planner_B.nx_j = 0;
8924 cartesian_trajectory_planner_B.nx_j <=
8925 cartesian_trajectory_planner_B.inner;
8926 cartesian_trajectory_planner_B.nx_j++) {
8927 cartesian_trajectory_planner_B.aoffset =
8928 cartesian_trajectory_planner_B.nx_j * a->size[0] - 1;
8929 for (cartesian_trajectory_planner_B.j = 0;
8930 cartesian_trajectory_planner_B.j <=
8931 cartesian_trajectory_planner_B.m_k;
8932 cartesian_trajectory_planner_B.j++) {
8933 alpha->data[cartesian_trajectory_planner_B.j] += a->data
8934 [(cartesian_trajectory_planner_B.aoffset +
8935 cartesian_trajectory_planner_B.j) + 1] * grad->
8936 data[cartesian_trajectory_planner_B.nx_j];
8937 }
8938 }
8939 }
8940
8941 for (cartesian_trajectory_planner_B.b_i = 0;
8942 cartesian_trajectory_planner_B.b_i < 6;
8943 cartesian_trajectory_planner_B.b_i++) {
8944 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i] =
8945 0.0;
8946 for (cartesian_trajectory_planner_B.idx = 0;
8947 cartesian_trajectory_planner_B.idx < 6;
8948 cartesian_trajectory_planner_B.idx++) {
8949 cartesian_trajectory_planner_B.b_gamma =
8950 cartesian_trajectory_planner_B.H[6 *
8951 cartesian_trajectory_planner_B.idx +
8952 cartesian_trajectory_planner_B.b_i] * grad->
8953 data[cartesian_trajectory_planner_B.idx] +
8954 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
8955 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
8956 = cartesian_trajectory_planner_B.b_gamma;
8957 }
8958 }
8959
8960 if (DampedBFGSwGradientProjection_a(obj,
8961 cartesian_trajectory_planner_B.Hg, alpha)) {
8962 *exitFlag = LocalMinimumFound;
8963 args = obj->ExtraArgs;
8964 for (cartesian_trajectory_planner_B.b_i = 0;
8965 cartesian_trajectory_planner_B.b_i < 36;
8966 cartesian_trajectory_planner_B.b_i++) {
8967 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
8968 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
8969 }
8970
8971 cartesian_trajectory_planner_B.b_i = grad->size[0];
8972 grad->size[0] = args->ErrTemp->size[0];
8973 cartes_emxEnsureCapacity_real_T(grad,
8974 cartesian_trajectory_planner_B.b_i);
8975 cartesian_trajectory_planner_B.i_h = args->ErrTemp->size[0];
8976 for (cartesian_trajectory_planner_B.b_i = 0;
8977 cartesian_trajectory_planner_B.b_i <
8978 cartesian_trajectory_planner_B.i_h;
8979 cartesian_trajectory_planner_B.b_i++) {
8980 grad->data[cartesian_trajectory_planner_B.b_i] = args->ErrTemp->
8981 data[cartesian_trajectory_planner_B.b_i];
8982 }
8983
8984 for (cartesian_trajectory_planner_B.b_i = 0;
8985 cartesian_trajectory_planner_B.b_i < 6;
8986 cartesian_trajectory_planner_B.b_i++) {
8987 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
8988 = 0.0;
8989 for (cartesian_trajectory_planner_B.idx = 0;
8990 cartesian_trajectory_planner_B.idx < 6;
8991 cartesian_trajectory_planner_B.idx++) {
8992 cartesian_trajectory_planner_B.A_d =
8993 cartesian_trajectory_planner_B.unusedU0[6 *
8994 cartesian_trajectory_planner_B.idx +
8995 cartesian_trajectory_planner_B.b_i] * grad->
8996 data[cartesian_trajectory_planner_B.idx] +
8997 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
8998 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
8999 = cartesian_trajectory_planner_B.A_d;
9000 }
9001 }
9002
9003 *err = cartesian_trajectory_pla_norm_a
9004 (cartesian_trajectory_planner_B.x);
9005 *iter = static_cast<real_T>(cartesian_trajectory_planner_B.g_idx_0) +
9006 1.0;
9007 exitg2 = 1;
9008 } else {
9009 guard1 = false;
9010 guard2 = false;
9011 if (obj->ConstraintsOn && ((A->size[0] != 0) && (A->size[1] != 0))) {
9012 cartesian_trajectory_planner_B.m_k = A->size[1] - 1;
9013 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
9014 cartesian_trajectory_planner_B.n_b = A->size[1] - 1;
9015 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
9016 AIn->size[0] = A->size[1];
9017 AIn->size[1] = A->size[1];
9018 cartes_emxEnsureCapacity_real_T(AIn,
9019 cartesian_trajectory_planner_B.b_i);
9020 for (cartesian_trajectory_planner_B.idx = 0;
9021 cartesian_trajectory_planner_B.idx <=
9022 cartesian_trajectory_planner_B.n_b;
9023 cartesian_trajectory_planner_B.idx++) {
9024 cartesian_trajectory_planner_B.coffset =
9025 (cartesian_trajectory_planner_B.m_k + 1) *
9026 cartesian_trajectory_planner_B.idx - 1;
9027 cartesian_trajectory_planner_B.boffset =
9028 cartesian_trajectory_planner_B.idx * A->size[0] - 1;
9029 for (cartesian_trajectory_planner_B.b_i = 0;
9030 cartesian_trajectory_planner_B.b_i <=
9031 cartesian_trajectory_planner_B.m_k;
9032 cartesian_trajectory_planner_B.b_i++) {
9033 AIn->data[(cartesian_trajectory_planner_B.coffset +
9034 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
9035 }
9036
9037 for (cartesian_trajectory_planner_B.nx_j = 0;
9038 cartesian_trajectory_planner_B.nx_j <=
9039 cartesian_trajectory_planner_B.inner;
9040 cartesian_trajectory_planner_B.nx_j++) {
9041 cartesian_trajectory_planner_B.s_i = A->data
9042 [(cartesian_trajectory_planner_B.boffset +
9043 cartesian_trajectory_planner_B.nx_j) + 1];
9044 for (cartesian_trajectory_planner_B.j = 0;
9045 cartesian_trajectory_planner_B.j <=
9046 cartesian_trajectory_planner_B.m_k;
9047 cartesian_trajectory_planner_B.j++) {
9048 cartesian_trajectory_planner_B.b_i =
9049 (cartesian_trajectory_planner_B.coffset +
9050 cartesian_trajectory_planner_B.j) + 1;
9051 AIn->data[cartesian_trajectory_planner_B.b_i] += A->
9052 data[cartesian_trajectory_planner_B.j * A->size[0] +
9053 cartesian_trajectory_planner_B.nx_j] *
9054 cartesian_trajectory_planner_B.s_i;
9055 }
9056 }
9057 }
9058
9059 cartesian_trajectory_planne_inv(AIn, a);
9060 cartesian_trajectory_plann_diag(a, L);
9061 cartesian_trajectory_pl_sqrt_as(L);
9062 cartesian_trajectory_planner_B.b_i = alpha->size[0];
9063 cartes_emxEnsureCapacity_real_T(alpha,
9064 cartesian_trajectory_planner_B.b_i);
9065 cartesian_trajectory_planner_B.i_h = alpha->size[0];
9066 for (cartesian_trajectory_planner_B.b_i = 0;
9067 cartesian_trajectory_planner_B.b_i <
9068 cartesian_trajectory_planner_B.i_h;
9069 cartesian_trajectory_planner_B.b_i++) {
9070 alpha->data[cartesian_trajectory_planner_B.b_i] /= L->
9071 data[cartesian_trajectory_planner_B.b_i];
9072 }
9073
9074 cartesian_trajectory_planner_B.n_b = alpha->size[0];
9075 if (alpha->size[0] <= 2) {
9076 if (alpha->size[0] == 1) {
9077 cartesian_trajectory_planner_B.s_i = alpha->data[0];
9078 cartesian_trajectory_planner_B.idxl = 0;
9079 } else if ((alpha->data[0] < alpha->data[1]) || (rtIsNaN
9080 (alpha->data[0]) && (!rtIsNaN(alpha->data[1])))) {
9081 cartesian_trajectory_planner_B.s_i = alpha->data[1];
9082 cartesian_trajectory_planner_B.idxl = 1;
9083 } else {
9084 cartesian_trajectory_planner_B.s_i = alpha->data[0];
9085 cartesian_trajectory_planner_B.idxl = 0;
9086 }
9087 } else {
9088 if (!rtIsNaN(alpha->data[0])) {
9089 cartesian_trajectory_planner_B.idxl = 1;
9090 } else {
9091 cartesian_trajectory_planner_B.idxl = 0;
9092 cartesian_trajectory_planner_B.b_i = 2;
9093 exitg3 = false;
9094 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i <=
9095 alpha->size[0])) {
9096 if (!rtIsNaN(alpha->data[cartesian_trajectory_planner_B.b_i -
9097 1])) {
9098 cartesian_trajectory_planner_B.idxl =
9099 cartesian_trajectory_planner_B.b_i;
9100 exitg3 = true;
9101 } else {
9102 cartesian_trajectory_planner_B.b_i++;
9103 }
9104 }
9105 }
9106
9107 if (cartesian_trajectory_planner_B.idxl == 0) {
9108 cartesian_trajectory_planner_B.s_i = alpha->data[0];
9109 } else {
9110 cartesian_trajectory_planner_B.s_i = alpha->
9111 data[cartesian_trajectory_planner_B.idxl - 1];
9112 cartesian_trajectory_planner_B.nx_j =
9113 cartesian_trajectory_planner_B.idxl;
9114 for (cartesian_trajectory_planner_B.b_i =
9115 cartesian_trajectory_planner_B.idxl + 1;
9116 cartesian_trajectory_planner_B.b_i <=
9117 cartesian_trajectory_planner_B.n_b;
9118 cartesian_trajectory_planner_B.b_i++) {
9119 if (cartesian_trajectory_planner_B.s_i < alpha->
9120 data[cartesian_trajectory_planner_B.b_i - 1]) {
9121 cartesian_trajectory_planner_B.s_i = alpha->
9122 data[cartesian_trajectory_planner_B.b_i - 1];
9123 cartesian_trajectory_planner_B.nx_j =
9124 cartesian_trajectory_planner_B.b_i;
9125 }
9126 }
9127
9128 cartesian_trajectory_planner_B.idxl =
9129 cartesian_trajectory_planner_B.nx_j - 1;
9130 }
9131 }
9132
9133 if (cartesian_trajectory_pla_norm_a
9134 (cartesian_trajectory_planner_B.Hg) < 0.5 *
9135 cartesian_trajectory_planner_B.s_i) {
9136 cartesian_trajectory_planner_B.nx_j = activeSet->size[0];
9137 cartesian_trajectory_planner_B.idx = 0;
9138 cartesian_trajectory_planner_B.b_i = ii->size[0];
9139 ii->size[0] = activeSet->size[0];
9140 carte_emxEnsureCapacity_int32_T(ii,
9141 cartesian_trajectory_planner_B.b_i);
9142 cartesian_trajectory_planner_B.b_i = 1;
9143 exitg3 = false;
9144 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i - 1 <=
9145 cartesian_trajectory_planner_B.nx_j - 1)) {
9146 if (activeSet->data[cartesian_trajectory_planner_B.b_i - 1]) {
9147 cartesian_trajectory_planner_B.idx++;
9148 ii->data[cartesian_trajectory_planner_B.idx - 1] =
9149 cartesian_trajectory_planner_B.b_i;
9150 if (cartesian_trajectory_planner_B.idx >=
9151 cartesian_trajectory_planner_B.nx_j) {
9152 exitg3 = true;
9153 } else {
9154 cartesian_trajectory_planner_B.b_i++;
9155 }
9156 } else {
9157 cartesian_trajectory_planner_B.b_i++;
9158 }
9159 }
9160
9161 if (activeSet->size[0] == 1) {
9162 if (cartesian_trajectory_planner_B.idx == 0) {
9163 ii->size[0] = 0;
9164 }
9165 } else {
9166 if (1 > cartesian_trajectory_planner_B.idx) {
9167 cartesian_trajectory_planner_B.idx = 0;
9168 }
9169
9170 cartesian_trajectory_planner_B.b_i = ii_1->size[0];
9171 ii_1->size[0] = cartesian_trajectory_planner_B.idx;
9172 carte_emxEnsureCapacity_int32_T(ii_1,
9173 cartesian_trajectory_planner_B.b_i);
9174 for (cartesian_trajectory_planner_B.b_i = 0;
9175 cartesian_trajectory_planner_B.b_i <
9176 cartesian_trajectory_planner_B.idx;
9177 cartesian_trajectory_planner_B.b_i++) {
9178 ii_1->data[cartesian_trajectory_planner_B.b_i] = ii->
9179 data[cartesian_trajectory_planner_B.b_i];
9180 }
9181
9182 cartesian_trajectory_planner_B.b_i = ii->size[0];
9183 ii->size[0] = ii_1->size[0];
9184 carte_emxEnsureCapacity_int32_T(ii,
9185 cartesian_trajectory_planner_B.b_i);
9186 cartesian_trajectory_planner_B.i_h = ii_1->size[0];
9187 for (cartesian_trajectory_planner_B.b_i = 0;
9188 cartesian_trajectory_planner_B.b_i <
9189 cartesian_trajectory_planner_B.i_h;
9190 cartesian_trajectory_planner_B.b_i++) {
9191 ii->data[cartesian_trajectory_planner_B.b_i] = ii_1->
9192 data[cartesian_trajectory_planner_B.b_i];
9193 }
9194 }
9195
9196 cartesian_trajectory_planner_B.b_i = alpha->size[0];
9197 alpha->size[0] = ii->size[0];
9198 cartes_emxEnsureCapacity_real_T(alpha,
9199 cartesian_trajectory_planner_B.b_i);
9200 cartesian_trajectory_planner_B.i_h = ii->size[0];
9201 for (cartesian_trajectory_planner_B.b_i = 0;
9202 cartesian_trajectory_planner_B.b_i <
9203 cartesian_trajectory_planner_B.i_h;
9204 cartesian_trajectory_planner_B.b_i++) {
9205 alpha->data[cartesian_trajectory_planner_B.b_i] = ii->
9206 data[cartesian_trajectory_planner_B.b_i];
9207 }
9208
9209 activeSet->data[static_cast<int32_T>(alpha->
9210 data[cartesian_trajectory_planner_B.idxl]) - 1] = false;
9211 cartesian_trajectory_planner_B.nx_j = activeSet->size[0] - 1;
9212 cartesian_trajectory_planner_B.idx = 0;
9213 for (cartesian_trajectory_planner_B.b_i = 0;
9214 cartesian_trajectory_planner_B.b_i <=
9215 cartesian_trajectory_planner_B.nx_j;
9216 cartesian_trajectory_planner_B.b_i++) {
9217 if (activeSet->data[cartesian_trajectory_planner_B.b_i]) {
9218 cartesian_trajectory_planner_B.idx++;
9219 }
9220 }
9221
9222 cartesian_trajectory_planner_B.b_i = eb->size[0];
9223 eb->size[0] = cartesian_trajectory_planner_B.idx;
9224 carte_emxEnsureCapacity_int32_T(eb,
9225 cartesian_trajectory_planner_B.b_i);
9226 cartesian_trajectory_planner_B.idx = 0;
9227 for (cartesian_trajectory_planner_B.b_i = 0;
9228 cartesian_trajectory_planner_B.b_i <=
9229 cartesian_trajectory_planner_B.nx_j;
9230 cartesian_trajectory_planner_B.b_i++) {
9231 if (activeSet->data[cartesian_trajectory_planner_B.b_i]) {
9232 eb->data[cartesian_trajectory_planner_B.idx] =
9233 cartesian_trajectory_planner_B.b_i + 1;
9234 cartesian_trajectory_planner_B.idx++;
9235 }
9236 }
9237
9238 cartesian_trajectory_planner_B.i_h = obj->ConstraintMatrix->size[0];
9239 cartesian_trajectory_planner_B.b_i = A->size[0] * A->size[1];
9240 A->size[0] = cartesian_trajectory_planner_B.i_h;
9241 A->size[1] = eb->size[0];
9242 cartes_emxEnsureCapacity_real_T(A,
9243 cartesian_trajectory_planner_B.b_i);
9244 cartesian_trajectory_planner_B.n_b = eb->size[0];
9245 for (cartesian_trajectory_planner_B.b_i = 0;
9246 cartesian_trajectory_planner_B.b_i <
9247 cartesian_trajectory_planner_B.n_b;
9248 cartesian_trajectory_planner_B.b_i++) {
9249 for (cartesian_trajectory_planner_B.idx = 0;
9250 cartesian_trajectory_planner_B.idx <
9251 cartesian_trajectory_planner_B.i_h;
9252 cartesian_trajectory_planner_B.idx++) {
9253 A->data[cartesian_trajectory_planner_B.idx + A->size[0] *
9254 cartesian_trajectory_planner_B.b_i] = obj->
9255 ConstraintMatrix->data[(eb->
9256 data[cartesian_trajectory_planner_B.b_i] - 1) *
9257 obj->ConstraintMatrix->size[0] +
9258 cartesian_trajectory_planner_B.idx];
9259 }
9260 }
9261
9262 cartesian_trajectory_planner_B.m_k = A->size[1] - 1;
9263 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
9264 cartesian_trajectory_planner_B.n_b = A->size[1] - 1;
9265 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
9266 AIn->size[0] = A->size[1];
9267 AIn->size[1] = A->size[1];
9268 cartes_emxEnsureCapacity_real_T(AIn,
9269 cartesian_trajectory_planner_B.b_i);
9270 for (cartesian_trajectory_planner_B.idx = 0;
9271 cartesian_trajectory_planner_B.idx <=
9272 cartesian_trajectory_planner_B.n_b;
9273 cartesian_trajectory_planner_B.idx++) {
9274 cartesian_trajectory_planner_B.coffset =
9275 (cartesian_trajectory_planner_B.m_k + 1) *
9276 cartesian_trajectory_planner_B.idx - 1;
9277 cartesian_trajectory_planner_B.boffset =
9278 cartesian_trajectory_planner_B.idx * A->size[0] - 1;
9279 for (cartesian_trajectory_planner_B.b_i = 0;
9280 cartesian_trajectory_planner_B.b_i <=
9281 cartesian_trajectory_planner_B.m_k;
9282 cartesian_trajectory_planner_B.b_i++) {
9283 AIn->data[(cartesian_trajectory_planner_B.coffset +
9284 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
9285 }
9286
9287 for (cartesian_trajectory_planner_B.nx_j = 0;
9288 cartesian_trajectory_planner_B.nx_j <=
9289 cartesian_trajectory_planner_B.inner;
9290 cartesian_trajectory_planner_B.nx_j++) {
9291 cartesian_trajectory_planner_B.s_i = A->data
9292 [(cartesian_trajectory_planner_B.boffset +
9293 cartesian_trajectory_planner_B.nx_j) + 1];
9294 for (cartesian_trajectory_planner_B.j = 0;
9295 cartesian_trajectory_planner_B.j <=
9296 cartesian_trajectory_planner_B.m_k;
9297 cartesian_trajectory_planner_B.j++) {
9298 cartesian_trajectory_planner_B.b_i =
9299 (cartesian_trajectory_planner_B.coffset +
9300 cartesian_trajectory_planner_B.j) + 1;
9301 AIn->data[cartesian_trajectory_planner_B.b_i] += A->
9302 data[cartesian_trajectory_planner_B.j * A->size[0] +
9303 cartesian_trajectory_planner_B.nx_j] *
9304 cartesian_trajectory_planner_B.s_i;
9305 }
9306 }
9307 }
9308
9309 cartesian_trajectory_planner_B.b_i = A_3->size[0] * A_3->size[1];
9310 A_3->size[0] = A->size[1];
9311 A_3->size[1] = A->size[0];
9312 cartes_emxEnsureCapacity_real_T(A_3,
9313 cartesian_trajectory_planner_B.b_i);
9314 cartesian_trajectory_planner_B.i_h = A->size[0];
9315 for (cartesian_trajectory_planner_B.b_i = 0;
9316 cartesian_trajectory_planner_B.b_i <
9317 cartesian_trajectory_planner_B.i_h;
9318 cartesian_trajectory_planner_B.b_i++) {
9319 cartesian_trajectory_planner_B.n_b = A->size[1];
9320 for (cartesian_trajectory_planner_B.idx = 0;
9321 cartesian_trajectory_planner_B.idx <
9322 cartesian_trajectory_planner_B.n_b;
9323 cartesian_trajectory_planner_B.idx++) {
9324 A_3->data[cartesian_trajectory_planner_B.idx + A_3->size[0] *
9325 cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
9326 cartesian_trajectory_planner_B.idx +
9327 cartesian_trajectory_planner_B.b_i];
9328 }
9329 }
9330
9331 cartesian_trajectory_p_mldivide(AIn, A_3, a);
9332 cartesian_trajectory_planner_B.m_k = A->size[0] - 1;
9333 cartesian_trajectory_planner_B.inner = A->size[1] - 1;
9334 cartesian_trajectory_planner_B.n_b = a->size[1] - 1;
9335 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
9336 AIn->size[0] = A->size[0];
9337 AIn->size[1] = a->size[1];
9338 cartes_emxEnsureCapacity_real_T(AIn,
9339 cartesian_trajectory_planner_B.b_i);
9340 for (cartesian_trajectory_planner_B.idx = 0;
9341 cartesian_trajectory_planner_B.idx <=
9342 cartesian_trajectory_planner_B.n_b;
9343 cartesian_trajectory_planner_B.idx++) {
9344 cartesian_trajectory_planner_B.coffset =
9345 (cartesian_trajectory_planner_B.m_k + 1) *
9346 cartesian_trajectory_planner_B.idx - 1;
9347 cartesian_trajectory_planner_B.boffset =
9348 cartesian_trajectory_planner_B.idx * a->size[0] - 1;
9349 for (cartesian_trajectory_planner_B.b_i = 0;
9350 cartesian_trajectory_planner_B.b_i <=
9351 cartesian_trajectory_planner_B.m_k;
9352 cartesian_trajectory_planner_B.b_i++) {
9353 AIn->data[(cartesian_trajectory_planner_B.coffset +
9354 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
9355 }
9356
9357 for (cartesian_trajectory_planner_B.nx_j = 0;
9358 cartesian_trajectory_planner_B.nx_j <=
9359 cartesian_trajectory_planner_B.inner;
9360 cartesian_trajectory_planner_B.nx_j++) {
9361 cartesian_trajectory_planner_B.aoffset =
9362 cartesian_trajectory_planner_B.nx_j * A->size[0] - 1;
9363 cartesian_trajectory_planner_B.s_i = a->data
9364 [(cartesian_trajectory_planner_B.boffset +
9365 cartesian_trajectory_planner_B.nx_j) + 1];
9366 for (cartesian_trajectory_planner_B.j = 0;
9367 cartesian_trajectory_planner_B.j <=
9368 cartesian_trajectory_planner_B.m_k;
9369 cartesian_trajectory_planner_B.j++) {
9370 cartesian_trajectory_planner_B.i_h =
9371 cartesian_trajectory_planner_B.j + 1;
9372 cartesian_trajectory_planner_B.b_i =
9373 cartesian_trajectory_planner_B.coffset +
9374 cartesian_trajectory_planner_B.i_h;
9375 AIn->data[cartesian_trajectory_planner_B.b_i] += A->
9376 data[cartesian_trajectory_planner_B.aoffset +
9377 cartesian_trajectory_planner_B.i_h] *
9378 cartesian_trajectory_planner_B.s_i;
9379 }
9380 }
9381 }
9382
9383 for (cartesian_trajectory_planner_B.b_i = 0;
9384 cartesian_trajectory_planner_B.b_i < 36;
9385 cartesian_trajectory_planner_B.b_i++) {
9386 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.b_i]
9387 =
9388 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
9389 - AIn->data[cartesian_trajectory_planner_B.b_i];
9390 }
9391
9392 cartesian_trajectory_planner_B.s_i = alpha->
9393 data[cartesian_trajectory_planner_B.idxl];
9394 cartesian_trajectory_planner_B.b_i = static_cast<int32_T>
9395 (cartesian_trajectory_planner_B.s_i);
9396 cartesian_trajectory_planner_B.i_h = obj->ConstraintMatrix->size[0];
9397 cartesian_trajectory_planner_B.idx = alpha->size[0];
9398 alpha->size[0] = cartesian_trajectory_planner_B.i_h;
9399 cartes_emxEnsureCapacity_real_T(alpha,
9400 cartesian_trajectory_planner_B.idx);
9401 for (cartesian_trajectory_planner_B.idx = 0;
9402 cartesian_trajectory_planner_B.idx <
9403 cartesian_trajectory_planner_B.i_h;
9404 cartesian_trajectory_planner_B.idx++) {
9405 alpha->data[cartesian_trajectory_planner_B.idx] =
9406 obj->ConstraintMatrix->data
9407 [(cartesian_trajectory_planner_B.b_i - 1) *
9408 obj->ConstraintMatrix->size[0] +
9409 cartesian_trajectory_planner_B.idx];
9410 }
9411
9412 cartesian_trajectory_planner_B.b_i = alpha_0->size[0] *
9413 alpha_0->size[1];
9414 alpha_0->size[0] = 1;
9415 alpha_0->size[1] = alpha->size[0];
9416 cartes_emxEnsureCapacity_real_T(alpha_0,
9417 cartesian_trajectory_planner_B.b_i);
9418 cartesian_trajectory_planner_B.i_h = alpha->size[0];
9419 for (cartesian_trajectory_planner_B.b_i = 0;
9420 cartesian_trajectory_planner_B.b_i <
9421 cartesian_trajectory_planner_B.i_h;
9422 cartesian_trajectory_planner_B.b_i++) {
9423 alpha_0->data[cartesian_trajectory_planner_B.b_i] = alpha->
9424 data[cartesian_trajectory_planner_B.b_i];
9425 }
9426
9427 cartesian_trajectory_planner_B.s_i = 0.0;
9428 for (cartesian_trajectory_planner_B.b_i = 0;
9429 cartesian_trajectory_planner_B.b_i < 6;
9430 cartesian_trajectory_planner_B.b_i++) {
9431 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
9432 = 0.0;
9433 for (cartesian_trajectory_planner_B.idx = 0;
9434 cartesian_trajectory_planner_B.idx < 6;
9435 cartesian_trajectory_planner_B.idx++) {
9436 cartesian_trajectory_planner_B.b_gamma =
9437 cartesian_trajectory_planner_B.P[6 *
9438 cartesian_trajectory_planner_B.b_i +
9439 cartesian_trajectory_planner_B.idx] * alpha_0->
9440 data[cartesian_trajectory_planner_B.idx] +
9441 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
9442 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
9443 = cartesian_trajectory_planner_B.b_gamma;
9444 }
9445
9446 cartesian_trajectory_planner_B.s_i +=
9447 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
9448 * alpha->data[cartesian_trajectory_planner_B.b_i];
9449 }
9450
9451 cartesian_trajectory_planner_B.s_i = 1.0 /
9452 cartesian_trajectory_planner_B.s_i;
9453 for (cartesian_trajectory_planner_B.b_i = 0;
9454 cartesian_trajectory_planner_B.b_i < 36;
9455 cartesian_trajectory_planner_B.b_i++) {
9456 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.b_i]
9457 = cartesian_trajectory_planner_B.s_i *
9458 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.b_i];
9459 }
9460
9461 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
9462 AIn->size[0] = alpha->size[0];
9463 AIn->size[1] = alpha->size[0];
9464 cartes_emxEnsureCapacity_real_T(AIn,
9465 cartesian_trajectory_planner_B.b_i);
9466 cartesian_trajectory_planner_B.i_h = alpha->size[0];
9467 for (cartesian_trajectory_planner_B.b_i = 0;
9468 cartesian_trajectory_planner_B.b_i <
9469 cartesian_trajectory_planner_B.i_h;
9470 cartesian_trajectory_planner_B.b_i++) {
9471 cartesian_trajectory_planner_B.n_b = alpha->size[0];
9472 for (cartesian_trajectory_planner_B.idx = 0;
9473 cartesian_trajectory_planner_B.idx <
9474 cartesian_trajectory_planner_B.n_b;
9475 cartesian_trajectory_planner_B.idx++) {
9476 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
9477 cartesian_trajectory_planner_B.b_i] = alpha->
9478 data[cartesian_trajectory_planner_B.idx] * alpha->
9479 data[cartesian_trajectory_planner_B.b_i];
9480 }
9481 }
9482
9483 cartesian_trajectory_planner_B.n_b = AIn->size[1] - 1;
9484 cartesian_trajectory_planner_B.b_i = unusedU1->size[0] *
9485 unusedU1->size[1];
9486 unusedU1->size[0] = 6;
9487 unusedU1->size[1] = AIn->size[1];
9488 cartes_emxEnsureCapacity_real_T(unusedU1,
9489 cartesian_trajectory_planner_B.b_i);
9490 for (cartesian_trajectory_planner_B.idx = 0;
9491 cartesian_trajectory_planner_B.idx <=
9492 cartesian_trajectory_planner_B.n_b;
9493 cartesian_trajectory_planner_B.idx++) {
9494 cartesian_trajectory_planner_B.coffset =
9495 cartesian_trajectory_planner_B.idx * 6 - 1;
9496 cartesian_trajectory_planner_B.boffset =
9497 cartesian_trajectory_planner_B.idx * AIn->size[0] - 1;
9498 for (cartesian_trajectory_planner_B.b_i = 0;
9499 cartesian_trajectory_planner_B.b_i < 6;
9500 cartesian_trajectory_planner_B.b_i++) {
9501 cartesian_trajectory_planner_B.s_i = 0.0;
9502 for (cartesian_trajectory_planner_B.nx_j = 0;
9503 cartesian_trajectory_planner_B.nx_j < 6;
9504 cartesian_trajectory_planner_B.nx_j++) {
9505 cartesian_trajectory_planner_B.s_i +=
9506 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.nx_j
9507 * 6 + cartesian_trajectory_planner_B.b_i] * AIn->data
9508 [(cartesian_trajectory_planner_B.boffset +
9509 cartesian_trajectory_planner_B.nx_j) + 1];
9510 }
9511
9512 unusedU1->data[(cartesian_trajectory_planner_B.coffset +
9513 cartesian_trajectory_planner_B.b_i) + 1] =
9514 cartesian_trajectory_planner_B.s_i;
9515 }
9516 }
9517
9518 for (cartesian_trajectory_planner_B.b_i = 0;
9519 cartesian_trajectory_planner_B.b_i < 6;
9520 cartesian_trajectory_planner_B.b_i++) {
9521 for (cartesian_trajectory_planner_B.idx = 0;
9522 cartesian_trajectory_planner_B.idx < 6;
9523 cartesian_trajectory_planner_B.idx++) {
9524 cartesian_trajectory_planner_B.s_i = 0.0;
9525 for (cartesian_trajectory_planner_B.i_h = 0;
9526 cartesian_trajectory_planner_B.i_h < 6;
9527 cartesian_trajectory_planner_B.i_h++) {
9528 cartesian_trajectory_planner_B.s_i += unusedU1->data[6 *
9529 cartesian_trajectory_planner_B.i_h +
9530 cartesian_trajectory_planner_B.b_i] *
9531 cartesian_trajectory_planner_B.P[6 *
9532 cartesian_trajectory_planner_B.idx +
9533 cartesian_trajectory_planner_B.i_h];
9534 }
9535
9536 cartesian_trajectory_planner_B.idxl = 6 *
9537 cartesian_trajectory_planner_B.idx +
9538 cartesian_trajectory_planner_B.b_i;
9539 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.idxl]
9540 += cartesian_trajectory_planner_B.s_i;
9541 }
9542 }
9543
9544 cartesian_trajectory_planner_B.g_idx_0++;
9545 } else {
9546 guard2 = true;
9547 }
9548 } else {
9549 guard2 = true;
9550 }
9551
9552 if (guard2) {
9553 for (cartesian_trajectory_planner_B.b_i = 0;
9554 cartesian_trajectory_planner_B.b_i < 6;
9555 cartesian_trajectory_planner_B.b_i++) {
9556 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
9557 =
9558 -cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
9559 }
9560
9561 cartesian_trajectory_planner_B.idxl = -1;
9562 if (obj->ConstraintsOn) {
9563 cartesian_trajectory_planner_B.b_i = x->size[0];
9564 x->size[0] = activeSet->size[0];
9565 car_emxEnsureCapacity_boolean_T(x,
9566 cartesian_trajectory_planner_B.b_i);
9567 cartesian_trajectory_planner_B.i_h = activeSet->size[0];
9568 for (cartesian_trajectory_planner_B.b_i = 0;
9569 cartesian_trajectory_planner_B.b_i <
9570 cartesian_trajectory_planner_B.i_h;
9571 cartesian_trajectory_planner_B.b_i++) {
9572 x->data[cartesian_trajectory_planner_B.b_i] = !activeSet->
9573 data[cartesian_trajectory_planner_B.b_i];
9574 }
9575
9576 if (cartesian_trajectory_planne_any(x)) {
9577 cartesian_trajectory_planner_B.nx_j = activeSet->size[0] - 1;
9578 cartesian_trajectory_planner_B.idx = 0;
9579 for (cartesian_trajectory_planner_B.b_i = 0;
9580 cartesian_trajectory_planner_B.b_i <=
9581 cartesian_trajectory_planner_B.nx_j;
9582 cartesian_trajectory_planner_B.b_i++) {
9583 if (!activeSet->data[cartesian_trajectory_planner_B.b_i]) {
9584 cartesian_trajectory_planner_B.idx++;
9585 }
9586 }
9587
9588 cartesian_trajectory_planner_B.b_i = cb->size[0];
9589 cb->size[0] = cartesian_trajectory_planner_B.idx;
9590 carte_emxEnsureCapacity_int32_T(cb,
9591 cartesian_trajectory_planner_B.b_i);
9592 cartesian_trajectory_planner_B.idx = 0;
9593 for (cartesian_trajectory_planner_B.b_i = 0;
9594 cartesian_trajectory_planner_B.b_i <=
9595 cartesian_trajectory_planner_B.nx_j;
9596 cartesian_trajectory_planner_B.b_i++) {
9597 if (!activeSet->data[cartesian_trajectory_planner_B.b_i]) {
9598 cb->data[cartesian_trajectory_planner_B.idx] =
9599 cartesian_trajectory_planner_B.b_i + 1;
9600 cartesian_trajectory_planner_B.idx++;
9601 }
9602 }
9603
9604 cartesian_trajectory_planner_B.b_i = alpha->size[0];
9605 alpha->size[0] = cb->size[0];
9606 cartes_emxEnsureCapacity_real_T(alpha,
9607 cartesian_trajectory_planner_B.b_i);
9608 cartesian_trajectory_planner_B.i_h = cb->size[0];
9609 for (cartesian_trajectory_planner_B.b_i = 0;
9610 cartesian_trajectory_planner_B.b_i <
9611 cartesian_trajectory_planner_B.i_h;
9612 cartesian_trajectory_planner_B.b_i++) {
9613 alpha->data[cartesian_trajectory_planner_B.b_i] =
9614 obj->ConstraintBound->data[cb->
9615 data[cartesian_trajectory_planner_B.b_i] - 1];
9616 }
9617
9618 cartesian_trajectory_planner_B.nx_j = activeSet->size[0] - 1;
9619 cartesian_trajectory_planner_B.idx = 0;
9620 for (cartesian_trajectory_planner_B.b_i = 0;
9621 cartesian_trajectory_planner_B.b_i <=
9622 cartesian_trajectory_planner_B.nx_j;
9623 cartesian_trajectory_planner_B.b_i++) {
9624 if (!activeSet->data[cartesian_trajectory_planner_B.b_i]) {
9625 cartesian_trajectory_planner_B.idx++;
9626 }
9627 }
9628
9629 cartesian_trajectory_planner_B.b_i = db->size[0];
9630 db->size[0] = cartesian_trajectory_planner_B.idx;
9631 carte_emxEnsureCapacity_int32_T(db,
9632 cartesian_trajectory_planner_B.b_i);
9633 cartesian_trajectory_planner_B.idx = 0;
9634 for (cartesian_trajectory_planner_B.b_i = 0;
9635 cartesian_trajectory_planner_B.b_i <=
9636 cartesian_trajectory_planner_B.nx_j;
9637 cartesian_trajectory_planner_B.b_i++) {
9638 if (!activeSet->data[cartesian_trajectory_planner_B.b_i]) {
9639 db->data[cartesian_trajectory_planner_B.idx] =
9640 cartesian_trajectory_planner_B.b_i + 1;
9641 cartesian_trajectory_planner_B.idx++;
9642 }
9643 }
9644
9645 cartesian_trajectory_planner_B.i_h = obj->ConstraintMatrix->
9646 size[0];
9647 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
9648 AIn->size[0] = cartesian_trajectory_planner_B.i_h;
9649 AIn->size[1] = db->size[0];
9650 cartes_emxEnsureCapacity_real_T(AIn,
9651 cartesian_trajectory_planner_B.b_i);
9652 cartesian_trajectory_planner_B.n_b = db->size[0];
9653 for (cartesian_trajectory_planner_B.b_i = 0;
9654 cartesian_trajectory_planner_B.b_i <
9655 cartesian_trajectory_planner_B.n_b;
9656 cartesian_trajectory_planner_B.b_i++) {
9657 for (cartesian_trajectory_planner_B.idx = 0;
9658 cartesian_trajectory_planner_B.idx <
9659 cartesian_trajectory_planner_B.i_h;
9660 cartesian_trajectory_planner_B.idx++) {
9661 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
9662 cartesian_trajectory_planner_B.b_i] =
9663 obj->ConstraintMatrix->data[(db->
9664 data[cartesian_trajectory_planner_B.b_i] - 1) *
9665 obj->ConstraintMatrix->size[0] +
9666 cartesian_trajectory_planner_B.idx];
9667 }
9668 }
9669
9670 cartesian_trajectory_planner_B.nx_j = x->size[0];
9671 cartesian_trajectory_planner_B.idx = 0;
9672 cartesian_trajectory_planner_B.b_i = ii->size[0];
9673 ii->size[0] = x->size[0];
9674 carte_emxEnsureCapacity_int32_T(ii,
9675 cartesian_trajectory_planner_B.b_i);
9676 cartesian_trajectory_planner_B.b_i = 1;
9677 exitg3 = false;
9678 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i - 1 <=
9679 cartesian_trajectory_planner_B.nx_j - 1)) {
9680 if (x->data[cartesian_trajectory_planner_B.b_i - 1]) {
9681 cartesian_trajectory_planner_B.idx++;
9682 ii->data[cartesian_trajectory_planner_B.idx - 1] =
9683 cartesian_trajectory_planner_B.b_i;
9684 if (cartesian_trajectory_planner_B.idx >=
9685 cartesian_trajectory_planner_B.nx_j) {
9686 exitg3 = true;
9687 } else {
9688 cartesian_trajectory_planner_B.b_i++;
9689 }
9690 } else {
9691 cartesian_trajectory_planner_B.b_i++;
9692 }
9693 }
9694
9695 if (x->size[0] == 1) {
9696 if (cartesian_trajectory_planner_B.idx == 0) {
9697 ii->size[0] = 0;
9698 }
9699 } else {
9700 if (1 > cartesian_trajectory_planner_B.idx) {
9701 cartesian_trajectory_planner_B.idx = 0;
9702 }
9703
9704 cartesian_trajectory_planner_B.b_i = ii_2->size[0];
9705 ii_2->size[0] = cartesian_trajectory_planner_B.idx;
9706 carte_emxEnsureCapacity_int32_T(ii_2,
9707 cartesian_trajectory_planner_B.b_i);
9708 for (cartesian_trajectory_planner_B.b_i = 0;
9709 cartesian_trajectory_planner_B.b_i <
9710 cartesian_trajectory_planner_B.idx;
9711 cartesian_trajectory_planner_B.b_i++) {
9712 ii_2->data[cartesian_trajectory_planner_B.b_i] = ii->
9713 data[cartesian_trajectory_planner_B.b_i];
9714 }
9715
9716 cartesian_trajectory_planner_B.b_i = ii->size[0];
9717 ii->size[0] = ii_2->size[0];
9718 carte_emxEnsureCapacity_int32_T(ii,
9719 cartesian_trajectory_planner_B.b_i);
9720 cartesian_trajectory_planner_B.i_h = ii_2->size[0];
9721 for (cartesian_trajectory_planner_B.b_i = 0;
9722 cartesian_trajectory_planner_B.b_i <
9723 cartesian_trajectory_planner_B.i_h;
9724 cartesian_trajectory_planner_B.b_i++) {
9725 ii->data[cartesian_trajectory_planner_B.b_i] = ii_2->
9726 data[cartesian_trajectory_planner_B.b_i];
9727 }
9728 }
9729
9730 cartesian_trajectory_planner_B.m_k = AIn->size[1] - 1;
9731 cartesian_trajectory_planner_B.inner = AIn->size[0] - 1;
9732 cartesian_trajectory_planner_B.b_i = L->size[0];
9733 L->size[0] = AIn->size[1];
9734 cartes_emxEnsureCapacity_real_T(L,
9735 cartesian_trajectory_planner_B.b_i);
9736 for (cartesian_trajectory_planner_B.b_i = 0;
9737 cartesian_trajectory_planner_B.b_i <=
9738 cartesian_trajectory_planner_B.m_k;
9739 cartesian_trajectory_planner_B.b_i++) {
9740 L->data[cartesian_trajectory_planner_B.b_i] = 0.0;
9741 }
9742
9743 for (cartesian_trajectory_planner_B.nx_j = 0;
9744 cartesian_trajectory_planner_B.nx_j <=
9745 cartesian_trajectory_planner_B.inner;
9746 cartesian_trajectory_planner_B.nx_j++) {
9747 for (cartesian_trajectory_planner_B.j = 0;
9748 cartesian_trajectory_planner_B.j <=
9749 cartesian_trajectory_planner_B.m_k;
9750 cartesian_trajectory_planner_B.j++) {
9751 L->data[cartesian_trajectory_planner_B.j] += AIn->
9752 data[cartesian_trajectory_planner_B.j * AIn->size[0] +
9753 cartesian_trajectory_planner_B.nx_j] *
9754 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.nx_j];
9755 }
9756 }
9757
9758 cartesian_trajectory_planner_B.m_k = AIn->size[1] - 1;
9759 cartesian_trajectory_planner_B.inner = AIn->size[0] - 1;
9760 cartesian_trajectory_planner_B.b_i = y->size[0];
9761 y->size[0] = AIn->size[1];
9762 cartes_emxEnsureCapacity_real_T(y,
9763 cartesian_trajectory_planner_B.b_i);
9764 for (cartesian_trajectory_planner_B.b_i = 0;
9765 cartesian_trajectory_planner_B.b_i <=
9766 cartesian_trajectory_planner_B.m_k;
9767 cartesian_trajectory_planner_B.b_i++) {
9768 y->data[cartesian_trajectory_planner_B.b_i] = 0.0;
9769 }
9770
9771 for (cartesian_trajectory_planner_B.nx_j = 0;
9772 cartesian_trajectory_planner_B.nx_j <=
9773 cartesian_trajectory_planner_B.inner;
9774 cartesian_trajectory_planner_B.nx_j++) {
9775 for (cartesian_trajectory_planner_B.j = 0;
9776 cartesian_trajectory_planner_B.j <=
9777 cartesian_trajectory_planner_B.m_k;
9778 cartesian_trajectory_planner_B.j++) {
9779 y->data[cartesian_trajectory_planner_B.j] += AIn->
9780 data[cartesian_trajectory_planner_B.j * AIn->size[0] +
9781 cartesian_trajectory_planner_B.nx_j] *
9782 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.nx_j];
9783 }
9784 }
9785
9786 cartesian_trajectory_planner_B.b_i = alpha->size[0];
9787 cartes_emxEnsureCapacity_real_T(alpha,
9788 cartesian_trajectory_planner_B.b_i);
9789 cartesian_trajectory_planner_B.i_h = alpha->size[0];
9790 for (cartesian_trajectory_planner_B.b_i = 0;
9791 cartesian_trajectory_planner_B.b_i <
9792 cartesian_trajectory_planner_B.i_h;
9793 cartesian_trajectory_planner_B.b_i++) {
9794 alpha->data[cartesian_trajectory_planner_B.b_i] = (alpha->
9795 data[cartesian_trajectory_planner_B.b_i] - L->
9796 data[cartesian_trajectory_planner_B.b_i]) / y->
9797 data[cartesian_trajectory_planner_B.b_i];
9798 }
9799
9800 cartesian_trajectory_planner_B.b_i = x->size[0];
9801 x->size[0] = alpha->size[0];
9802 car_emxEnsureCapacity_boolean_T(x,
9803 cartesian_trajectory_planner_B.b_i);
9804 cartesian_trajectory_planner_B.i_h = alpha->size[0];
9805 for (cartesian_trajectory_planner_B.b_i = 0;
9806 cartesian_trajectory_planner_B.b_i <
9807 cartesian_trajectory_planner_B.i_h;
9808 cartesian_trajectory_planner_B.b_i++) {
9809 x->data[cartesian_trajectory_planner_B.b_i] = (alpha->
9810 data[cartesian_trajectory_planner_B.b_i] > 0.0);
9811 }
9812
9813 cartesian_trajectory_planner_B.nx_j = x->size[0];
9814 cartesian_trajectory_planner_B.idx = 0;
9815 cartesian_trajectory_planner_B.b_i = ii_0->size[0];
9816 ii_0->size[0] = x->size[0];
9817 carte_emxEnsureCapacity_int32_T(ii_0,
9818 cartesian_trajectory_planner_B.b_i);
9819 cartesian_trajectory_planner_B.b_i = 1;
9820 exitg3 = false;
9821 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i - 1 <=
9822 cartesian_trajectory_planner_B.nx_j - 1)) {
9823 if (x->data[cartesian_trajectory_planner_B.b_i - 1]) {
9824 cartesian_trajectory_planner_B.idx++;
9825 ii_0->data[cartesian_trajectory_planner_B.idx - 1] =
9826 cartesian_trajectory_planner_B.b_i;
9827 if (cartesian_trajectory_planner_B.idx >=
9828 cartesian_trajectory_planner_B.nx_j) {
9829 exitg3 = true;
9830 } else {
9831 cartesian_trajectory_planner_B.b_i++;
9832 }
9833 } else {
9834 cartesian_trajectory_planner_B.b_i++;
9835 }
9836 }
9837
9838 if (x->size[0] == 1) {
9839 if (cartesian_trajectory_planner_B.idx == 0) {
9840 ii_0->size[0] = 0;
9841 }
9842 } else {
9843 if (1 > cartesian_trajectory_planner_B.idx) {
9844 cartesian_trajectory_planner_B.idx = 0;
9845 }
9846
9847 cartesian_trajectory_planner_B.b_i = ii_3->size[0];
9848 ii_3->size[0] = cartesian_trajectory_planner_B.idx;
9849 carte_emxEnsureCapacity_int32_T(ii_3,
9850 cartesian_trajectory_planner_B.b_i);
9851 for (cartesian_trajectory_planner_B.b_i = 0;
9852 cartesian_trajectory_planner_B.b_i <
9853 cartesian_trajectory_planner_B.idx;
9854 cartesian_trajectory_planner_B.b_i++) {
9855 ii_3->data[cartesian_trajectory_planner_B.b_i] = ii_0->
9856 data[cartesian_trajectory_planner_B.b_i];
9857 }
9858
9859 cartesian_trajectory_planner_B.b_i = ii_0->size[0];
9860 ii_0->size[0] = ii_3->size[0];
9861 carte_emxEnsureCapacity_int32_T(ii_0,
9862 cartesian_trajectory_planner_B.b_i);
9863 cartesian_trajectory_planner_B.i_h = ii_3->size[0];
9864 for (cartesian_trajectory_planner_B.b_i = 0;
9865 cartesian_trajectory_planner_B.b_i <
9866 cartesian_trajectory_planner_B.i_h;
9867 cartesian_trajectory_planner_B.b_i++) {
9868 ii_0->data[cartesian_trajectory_planner_B.b_i] = ii_3->
9869 data[cartesian_trajectory_planner_B.b_i];
9870 }
9871 }
9872
9873 cartesian_trajectory_planner_B.b_i = L->size[0];
9874 L->size[0] = ii_0->size[0];
9875 cartes_emxEnsureCapacity_real_T(L,
9876 cartesian_trajectory_planner_B.b_i);
9877 cartesian_trajectory_planner_B.i_h = ii_0->size[0];
9878 for (cartesian_trajectory_planner_B.b_i = 0;
9879 cartesian_trajectory_planner_B.b_i <
9880 cartesian_trajectory_planner_B.i_h;
9881 cartesian_trajectory_planner_B.b_i++) {
9882 L->data[cartesian_trajectory_planner_B.b_i] = ii_0->
9883 data[cartesian_trajectory_planner_B.b_i];
9884 }
9885
9886 if (L->size[0] != 0) {
9887 cartesian_trajectory_planner_B.nx_j = alpha->size[0] - 1;
9888 cartesian_trajectory_planner_B.idx = 0;
9889 for (cartesian_trajectory_planner_B.b_i = 0;
9890 cartesian_trajectory_planner_B.b_i <=
9891 cartesian_trajectory_planner_B.nx_j;
9892 cartesian_trajectory_planner_B.b_i++) {
9893 if (alpha->data[cartesian_trajectory_planner_B.b_i] > 0.0) {
9894 cartesian_trajectory_planner_B.idx++;
9895 }
9896 }
9897
9898 cartesian_trajectory_planner_B.b_i = fb->size[0];
9899 fb->size[0] = cartesian_trajectory_planner_B.idx;
9900 carte_emxEnsureCapacity_int32_T(fb,
9901 cartesian_trajectory_planner_B.b_i);
9902 cartesian_trajectory_planner_B.idx = 0;
9903 for (cartesian_trajectory_planner_B.b_i = 0;
9904 cartesian_trajectory_planner_B.b_i <=
9905 cartesian_trajectory_planner_B.nx_j;
9906 cartesian_trajectory_planner_B.b_i++) {
9907 if (alpha->data[cartesian_trajectory_planner_B.b_i] > 0.0) {
9908 fb->data[cartesian_trajectory_planner_B.idx] =
9909 cartesian_trajectory_planner_B.b_i + 1;
9910 cartesian_trajectory_planner_B.idx++;
9911 }
9912 }
9913
9914 cartesian_trajectory_planner_B.n_b = fb->size[0];
9915 if (fb->size[0] <= 2) {
9916 if (fb->size[0] == 1) {
9917 cartesian_trajectory_planner_B.s_i = alpha->data[fb->data
9918 [0] - 1];
9919 cartesian_trajectory_planner_B.idxl = 0;
9920 } else if ((alpha->data[fb->data[0] - 1] > alpha->data
9921 [fb->data[1] - 1]) || (rtIsNaN(alpha->data
9922 [fb->data[0] - 1]) && (!rtIsNaN(alpha->data
9923 [fb->data[1] - 1])))) {
9924 cartesian_trajectory_planner_B.s_i = alpha->data[fb->data
9925 [1] - 1];
9926 cartesian_trajectory_planner_B.idxl = 1;
9927 } else {
9928 cartesian_trajectory_planner_B.s_i = alpha->data[fb->data
9929 [0] - 1];
9930 cartesian_trajectory_planner_B.idxl = 0;
9931 }
9932 } else {
9933 if (!rtIsNaN(alpha->data[fb->data[0] - 1])) {
9934 cartesian_trajectory_planner_B.idxl = 1;
9935 } else {
9936 cartesian_trajectory_planner_B.idxl = 0;
9937 cartesian_trajectory_planner_B.b_i = 2;
9938 exitg3 = false;
9939 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i <=
9940 fb->size[0])) {
9941 if (!rtIsNaN(alpha->data[fb->
9942 data[cartesian_trajectory_planner_B.b_i - 1]
9943 - 1])) {
9944 cartesian_trajectory_planner_B.idxl =
9945 cartesian_trajectory_planner_B.b_i;
9946 exitg3 = true;
9947 } else {
9948 cartesian_trajectory_planner_B.b_i++;
9949 }
9950 }
9951 }
9952
9953 if (cartesian_trajectory_planner_B.idxl == 0) {
9954 cartesian_trajectory_planner_B.s_i = alpha->data[fb->data
9955 [0] - 1];
9956 } else {
9957 cartesian_trajectory_planner_B.s_i = alpha->data[fb->
9958 data[cartesian_trajectory_planner_B.idxl - 1] - 1];
9959 cartesian_trajectory_planner_B.nx_j =
9960 cartesian_trajectory_planner_B.idxl;
9961 for (cartesian_trajectory_planner_B.b_i =
9962 cartesian_trajectory_planner_B.idxl + 1;
9963 cartesian_trajectory_planner_B.b_i <=
9964 cartesian_trajectory_planner_B.n_b;
9965 cartesian_trajectory_planner_B.b_i++) {
9966 if (cartesian_trajectory_planner_B.s_i > alpha->data
9967 [fb->data[cartesian_trajectory_planner_B.b_i - 1] -
9968 1]) {
9969 cartesian_trajectory_planner_B.s_i = alpha->data
9970 [fb->data[cartesian_trajectory_planner_B.b_i - 1] -
9971 1];
9972 cartesian_trajectory_planner_B.nx_j =
9973 cartesian_trajectory_planner_B.b_i;
9974 }
9975 }
9976
9977 cartesian_trajectory_planner_B.idxl =
9978 cartesian_trajectory_planner_B.nx_j - 1;
9979 }
9980 }
9981
9982 cartesian_trajectory_planner_B.idxl = ii->data
9983 [static_cast<int32_T>(L->
9984 data[cartesian_trajectory_planner_B.idxl]) - 1];
9985 } else {
9986 cartesian_trajectory_planner_B.s_i = 0.0;
9987 }
9988 } else {
9989 cartesian_trajectory_planner_B.s_i = 0.0;
9990 }
9991 } else {
9992 cartesian_trajectory_planner_B.s_i = 0.0;
9993 }
9994
9995 if (cartesian_trajectory_planner_B.s_i > 0.0) {
9996 if (1.0 < cartesian_trajectory_planner_B.s_i) {
9997 cartesian_trajectory_planner_B.b_gamma = 1.0;
9998 } else {
9999 cartesian_trajectory_planner_B.b_gamma =
10000 cartesian_trajectory_planner_B.s_i;
10001 }
10002 } else {
10003 cartesian_trajectory_planner_B.b_gamma = 1.0;
10004 }
10005
10006 cartesian_trajectory_planner_B.beta = obj->ArmijoRuleBeta;
10007 cartesian_trajectory_planner_B.sigma = obj->ArmijoRuleSigma;
10008 for (cartesian_trajectory_planner_B.b_i = 0;
10009 cartesian_trajectory_planner_B.b_i < 6;
10010 cartesian_trajectory_planner_B.b_i++) {
10011 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10012 = cartesian_trajectory_planner_B.b_gamma *
10013 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
10014 + cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
10015 }
10016
10017 cartesian_IKHelpers_computeCost
10018 (cartesian_trajectory_planner_B.sNew_a, obj->ExtraArgs,
10019 &cartesian_trajectory_planner_B.costNew,
10020 cartesian_trajectory_planner_B.V, unusedU1, &c);
10021 obj->ExtraArgs = c;
10022 cartesian_trajectory_planner_B.m = 0.0;
10023 do {
10024 exitg1 = 0;
10025 for (cartesian_trajectory_planner_B.i_h = 0;
10026 cartesian_trajectory_planner_B.i_h < 6;
10027 cartesian_trajectory_planner_B.i_h++) {
10028 xSol[cartesian_trajectory_planner_B.i_h] =
10029 cartesian_trajectory_planner_B.b_gamma *
10030 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.i_h];
10031 }
10032
10033 cartesian_trajectory_planner_B.b_i = sigma->size[0] * sigma->size
10034 [1];
10035 sigma->size[0] = 1;
10036 sigma->size[1] = grad->size[0];
10037 cartes_emxEnsureCapacity_real_T(sigma,
10038 cartesian_trajectory_planner_B.b_i);
10039 cartesian_trajectory_planner_B.i_h = grad->size[0];
10040 for (cartesian_trajectory_planner_B.b_i = 0;
10041 cartesian_trajectory_planner_B.b_i <
10042 cartesian_trajectory_planner_B.i_h;
10043 cartesian_trajectory_planner_B.b_i++) {
10044 sigma->data[cartesian_trajectory_planner_B.b_i] =
10045 -cartesian_trajectory_planner_B.sigma * grad->
10046 data[cartesian_trajectory_planner_B.b_i];
10047 }
10048
10049 cartesian_trajectory_planner_B.sigma_g = 0.0;
10050 for (cartesian_trajectory_planner_B.b_i = 0;
10051 cartesian_trajectory_planner_B.b_i < 6;
10052 cartesian_trajectory_planner_B.b_i++) {
10053 cartesian_trajectory_planner_B.sigma_g += sigma->
10054 data[cartesian_trajectory_planner_B.b_i] *
10055 xSol[cartesian_trajectory_planner_B.b_i];
10056 }
10057
10058 if (cartesian_trajectory_planner_B.cost -
10059 cartesian_trajectory_planner_B.costNew <
10060 cartesian_trajectory_planner_B.sigma_g) {
10061 cartesian_trajectory_planner_B.flag =
10062 (cartesian_trajectory_planner_B.b_gamma < obj->StepTolerance);
10063 if (cartesian_trajectory_planner_B.flag) {
10064 for (cartesian_trajectory_planner_B.i_h = 0;
10065 cartesian_trajectory_planner_B.i_h < 6;
10066 cartesian_trajectory_planner_B.i_h++) {
10067 xSol[cartesian_trajectory_planner_B.i_h] =
10068 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_h];
10069 }
10070
10071 *exitFlag = StepSizeBelowMinimum;
10072 args = obj->ExtraArgs;
10073 for (cartesian_trajectory_planner_B.b_i = 0;
10074 cartesian_trajectory_planner_B.b_i < 36;
10075 cartesian_trajectory_planner_B.b_i++) {
10076 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
10077 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
10078 }
10079
10080 cartesian_trajectory_planner_B.b_i = grad->size[0];
10081 grad->size[0] = args->ErrTemp->size[0];
10082 cartes_emxEnsureCapacity_real_T(grad,
10083 cartesian_trajectory_planner_B.b_i);
10084 cartesian_trajectory_planner_B.i_h = args->ErrTemp->size[0];
10085 for (cartesian_trajectory_planner_B.b_i = 0;
10086 cartesian_trajectory_planner_B.b_i <
10087 cartesian_trajectory_planner_B.i_h;
10088 cartesian_trajectory_planner_B.b_i++) {
10089 grad->data[cartesian_trajectory_planner_B.b_i] =
10090 args->ErrTemp->data[cartesian_trajectory_planner_B.b_i];
10091 }
10092
10093 for (cartesian_trajectory_planner_B.b_i = 0;
10094 cartesian_trajectory_planner_B.b_i < 6;
10095 cartesian_trajectory_planner_B.b_i++) {
10096 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
10097 = 0.0;
10098 for (cartesian_trajectory_planner_B.idx = 0;
10099 cartesian_trajectory_planner_B.idx < 6;
10100 cartesian_trajectory_planner_B.idx++) {
10101 cartesian_trajectory_planner_B.A_d =
10102 cartesian_trajectory_planner_B.unusedU0[6 *
10103 cartesian_trajectory_planner_B.idx +
10104 cartesian_trajectory_planner_B.b_i] * grad->
10105 data[cartesian_trajectory_planner_B.idx] +
10106 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
10107 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
10108 = cartesian_trajectory_planner_B.A_d;
10109 }
10110 }
10111
10112 *err = cartesian_trajectory_pla_norm_a
10113 (cartesian_trajectory_planner_B.x);
10114 *iter = static_cast<real_T>
10115 (cartesian_trajectory_planner_B.g_idx_0) + 1.0;
10116 exitg1 = 1;
10117 } else {
10118 cartesian_trajectory_planner_B.b_gamma *=
10119 cartesian_trajectory_planner_B.beta;
10120 cartesian_trajectory_planner_B.m++;
10121 for (cartesian_trajectory_planner_B.b_i = 0;
10122 cartesian_trajectory_planner_B.b_i < 6;
10123 cartesian_trajectory_planner_B.b_i++) {
10124 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10125 = cartesian_trajectory_planner_B.b_gamma *
10126 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
10127 + cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
10128 }
10129
10130 cartesian_IKHelpers_computeCost
10131 (cartesian_trajectory_planner_B.sNew_a, obj->ExtraArgs,
10132 &cartesian_trajectory_planner_B.costNew,
10133 cartesian_trajectory_planner_B.V, unusedU1, &d);
10134 obj->ExtraArgs = d;
10135 }
10136 } else {
10137 for (cartesian_trajectory_planner_B.b_i = 0;
10138 cartesian_trajectory_planner_B.b_i < 6;
10139 cartesian_trajectory_planner_B.b_i++) {
10140 xSol[cartesian_trajectory_planner_B.b_i] +=
10141 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
10142 }
10143
10144 args = obj->ExtraArgs;
10145 cartesian_trajectory_planner_B.b_i = alpha->size[0];
10146 alpha->size[0] = args->GradTemp->size[0];
10147 cartes_emxEnsureCapacity_real_T(alpha,
10148 cartesian_trajectory_planner_B.b_i);
10149 cartesian_trajectory_planner_B.i_h = args->GradTemp->size[0];
10150 for (cartesian_trajectory_planner_B.b_i = 0;
10151 cartesian_trajectory_planner_B.b_i <
10152 cartesian_trajectory_planner_B.i_h;
10153 cartesian_trajectory_planner_B.b_i++) {
10154 alpha->data[cartesian_trajectory_planner_B.b_i] =
10155 args->GradTemp->data[cartesian_trajectory_planner_B.b_i];
10156 }
10157
10158 exitg1 = 2;
10159 }
10160 } while (exitg1 == 0);
10161
10162 if (exitg1 == 1) {
10163 exitg2 = 1;
10164 } else if ((cartesian_trajectory_planner_B.m == 0.0) && (fabs
10165 (cartesian_trajectory_planner_B.b_gamma -
10166 cartesian_trajectory_planner_B.s_i) <
10167 1.4901161193847656E-8)) {
10168 cartesian_trajectory_planner_B.i_h = obj->ConstraintMatrix->size[0];
10169 cartesian_trajectory_planner_B.b_i = grad->size[0];
10170 grad->size[0] = cartesian_trajectory_planner_B.i_h;
10171 cartes_emxEnsureCapacity_real_T(grad,
10172 cartesian_trajectory_planner_B.b_i);
10173 for (cartesian_trajectory_planner_B.b_i = 0;
10174 cartesian_trajectory_planner_B.b_i <
10175 cartesian_trajectory_planner_B.i_h;
10176 cartesian_trajectory_planner_B.b_i++) {
10177 grad->data[cartesian_trajectory_planner_B.b_i] =
10178 obj->ConstraintMatrix->data
10179 [(cartesian_trajectory_planner_B.idxl - 1) *
10180 obj->ConstraintMatrix->size[0] +
10181 cartesian_trajectory_planner_B.b_i];
10182 }
10183
10184 activeSet->data[cartesian_trajectory_planner_B.idxl - 1] = true;
10185 cartesian_trajectory_planner_B.nx_j = activeSet->size[0] - 1;
10186 cartesian_trajectory_planner_B.idx = 0;
10187 for (cartesian_trajectory_planner_B.b_i = 0;
10188 cartesian_trajectory_planner_B.b_i <=
10189 cartesian_trajectory_planner_B.nx_j;
10190 cartesian_trajectory_planner_B.b_i++) {
10191 if (activeSet->data[cartesian_trajectory_planner_B.b_i]) {
10192 cartesian_trajectory_planner_B.idx++;
10193 }
10194 }
10195
10196 cartesian_trajectory_planner_B.b_i = gb->size[0];
10197 gb->size[0] = cartesian_trajectory_planner_B.idx;
10198 carte_emxEnsureCapacity_int32_T(gb,
10199 cartesian_trajectory_planner_B.b_i);
10200 cartesian_trajectory_planner_B.idx = 0;
10201 for (cartesian_trajectory_planner_B.b_i = 0;
10202 cartesian_trajectory_planner_B.b_i <=
10203 cartesian_trajectory_planner_B.nx_j;
10204 cartesian_trajectory_planner_B.b_i++) {
10205 if (activeSet->data[cartesian_trajectory_planner_B.b_i]) {
10206 gb->data[cartesian_trajectory_planner_B.idx] =
10207 cartesian_trajectory_planner_B.b_i + 1;
10208 cartesian_trajectory_planner_B.idx++;
10209 }
10210 }
10211
10212 cartesian_trajectory_planner_B.i_h = obj->ConstraintMatrix->size[0];
10213 cartesian_trajectory_planner_B.b_i = A->size[0] * A->size[1];
10214 A->size[0] = cartesian_trajectory_planner_B.i_h;
10215 A->size[1] = gb->size[0];
10216 cartes_emxEnsureCapacity_real_T(A,
10217 cartesian_trajectory_planner_B.b_i);
10218 cartesian_trajectory_planner_B.n_b = gb->size[0];
10219 for (cartesian_trajectory_planner_B.b_i = 0;
10220 cartesian_trajectory_planner_B.b_i <
10221 cartesian_trajectory_planner_B.n_b;
10222 cartesian_trajectory_planner_B.b_i++) {
10223 for (cartesian_trajectory_planner_B.idx = 0;
10224 cartesian_trajectory_planner_B.idx <
10225 cartesian_trajectory_planner_B.i_h;
10226 cartesian_trajectory_planner_B.idx++) {
10227 A->data[cartesian_trajectory_planner_B.idx + A->size[0] *
10228 cartesian_trajectory_planner_B.b_i] = obj->
10229 ConstraintMatrix->data[(gb->
10230 data[cartesian_trajectory_planner_B.b_i] - 1) *
10231 obj->ConstraintMatrix->size[0] +
10232 cartesian_trajectory_planner_B.idx];
10233 }
10234 }
10235
10236 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
10237 AIn->size[0] = grad->size[0];
10238 AIn->size[1] = grad->size[0];
10239 cartes_emxEnsureCapacity_real_T(AIn,
10240 cartesian_trajectory_planner_B.b_i);
10241 cartesian_trajectory_planner_B.i_h = grad->size[0];
10242 for (cartesian_trajectory_planner_B.b_i = 0;
10243 cartesian_trajectory_planner_B.b_i <
10244 cartesian_trajectory_planner_B.i_h;
10245 cartesian_trajectory_planner_B.b_i++) {
10246 cartesian_trajectory_planner_B.n_b = grad->size[0];
10247 for (cartesian_trajectory_planner_B.idx = 0;
10248 cartesian_trajectory_planner_B.idx <
10249 cartesian_trajectory_planner_B.n_b;
10250 cartesian_trajectory_planner_B.idx++) {
10251 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
10252 cartesian_trajectory_planner_B.b_i] = grad->
10253 data[cartesian_trajectory_planner_B.idx] * grad->
10254 data[cartesian_trajectory_planner_B.b_i];
10255 }
10256 }
10257
10258 cartesian_trajectory_planner_B.m_k = AIn->size[0] - 1;
10259 cartesian_trajectory_planner_B.inner = AIn->size[1] - 1;
10260 cartesian_trajectory_planner_B.b_i = y_0->size[0] * y_0->size[1];
10261 y_0->size[0] = AIn->size[0];
10262 y_0->size[1] = 6;
10263 cartes_emxEnsureCapacity_real_T(y_0,
10264 cartesian_trajectory_planner_B.b_i);
10265 for (cartesian_trajectory_planner_B.idx = 0;
10266 cartesian_trajectory_planner_B.idx < 6;
10267 cartesian_trajectory_planner_B.idx++) {
10268 cartesian_trajectory_planner_B.coffset =
10269 (cartesian_trajectory_planner_B.m_k + 1) *
10270 cartesian_trajectory_planner_B.idx - 1;
10271 cartesian_trajectory_planner_B.boffset =
10272 cartesian_trajectory_planner_B.idx * 6 - 1;
10273 for (cartesian_trajectory_planner_B.b_i = 0;
10274 cartesian_trajectory_planner_B.b_i <=
10275 cartesian_trajectory_planner_B.m_k;
10276 cartesian_trajectory_planner_B.b_i++) {
10277 y_0->data[(cartesian_trajectory_planner_B.coffset +
10278 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
10279 }
10280
10281 for (cartesian_trajectory_planner_B.nx_j = 0;
10282 cartesian_trajectory_planner_B.nx_j <=
10283 cartesian_trajectory_planner_B.inner;
10284 cartesian_trajectory_planner_B.nx_j++) {
10285 cartesian_trajectory_planner_B.aoffset =
10286 cartesian_trajectory_planner_B.nx_j * AIn->size[0] - 1;
10287 cartesian_trajectory_planner_B.s_i =
10288 cartesian_trajectory_planner_B.H
10289 [(cartesian_trajectory_planner_B.boffset +
10290 cartesian_trajectory_planner_B.nx_j) + 1];
10291 for (cartesian_trajectory_planner_B.j = 0;
10292 cartesian_trajectory_planner_B.j <=
10293 cartesian_trajectory_planner_B.m_k;
10294 cartesian_trajectory_planner_B.j++) {
10295 cartesian_trajectory_planner_B.i_h =
10296 cartesian_trajectory_planner_B.j + 1;
10297 cartesian_trajectory_planner_B.b_i =
10298 cartesian_trajectory_planner_B.coffset +
10299 cartesian_trajectory_planner_B.i_h;
10300 y_0->data[cartesian_trajectory_planner_B.b_i] += AIn->
10301 data[cartesian_trajectory_planner_B.aoffset +
10302 cartesian_trajectory_planner_B.i_h] *
10303 cartesian_trajectory_planner_B.s_i;
10304 }
10305 }
10306 }
10307
10308 cartesian_trajectory_planner_B.b_i = grad_1->size[0] *
10309 grad_1->size[1];
10310 grad_1->size[0] = 1;
10311 grad_1->size[1] = grad->size[0];
10312 cartes_emxEnsureCapacity_real_T(grad_1,
10313 cartesian_trajectory_planner_B.b_i);
10314 cartesian_trajectory_planner_B.i_h = grad->size[0];
10315 for (cartesian_trajectory_planner_B.b_i = 0;
10316 cartesian_trajectory_planner_B.b_i <
10317 cartesian_trajectory_planner_B.i_h;
10318 cartesian_trajectory_planner_B.b_i++) {
10319 grad_1->data[cartesian_trajectory_planner_B.b_i] = grad->
10320 data[cartesian_trajectory_planner_B.b_i];
10321 }
10322
10323 cartesian_trajectory_planner_B.beta = 0.0;
10324 for (cartesian_trajectory_planner_B.b_i = 0;
10325 cartesian_trajectory_planner_B.b_i < 6;
10326 cartesian_trajectory_planner_B.b_i++) {
10327 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10328 = 0.0;
10329 for (cartesian_trajectory_planner_B.idx = 0;
10330 cartesian_trajectory_planner_B.idx < 6;
10331 cartesian_trajectory_planner_B.idx++) {
10332 cartesian_trajectory_planner_B.sigma =
10333 cartesian_trajectory_planner_B.H[6 *
10334 cartesian_trajectory_planner_B.b_i +
10335 cartesian_trajectory_planner_B.idx] * grad_1->
10336 data[cartesian_trajectory_planner_B.idx] +
10337 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i];
10338 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10339 = cartesian_trajectory_planner_B.sigma;
10340 }
10341
10342 cartesian_trajectory_planner_B.beta +=
10343 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10344 * grad->data[cartesian_trajectory_planner_B.b_i];
10345 }
10346
10347 cartesian_trajectory_planner_B.s_i = 1.0 /
10348 cartesian_trajectory_planner_B.beta;
10349 for (cartesian_trajectory_planner_B.b_i = 0;
10350 cartesian_trajectory_planner_B.b_i < 6;
10351 cartesian_trajectory_planner_B.b_i++) {
10352 for (cartesian_trajectory_planner_B.idx = 0;
10353 cartesian_trajectory_planner_B.idx < 6;
10354 cartesian_trajectory_planner_B.idx++) {
10355 cartesian_trajectory_planner_B.idxl =
10356 cartesian_trajectory_planner_B.b_i + 6 *
10357 cartesian_trajectory_planner_B.idx;
10358 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.idxl]
10359 = 0.0;
10360 for (cartesian_trajectory_planner_B.i_h = 0;
10361 cartesian_trajectory_planner_B.i_h < 6;
10362 cartesian_trajectory_planner_B.i_h++) {
10363 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.idxl]
10364 += cartesian_trajectory_planner_B.H[6 *
10365 cartesian_trajectory_planner_B.i_h +
10366 cartesian_trajectory_planner_B.b_i] * y_0->data[6 *
10367 cartesian_trajectory_planner_B.idx +
10368 cartesian_trajectory_planner_B.i_h];
10369 }
10370 }
10371 }
10372
10373 for (cartesian_trajectory_planner_B.b_i = 0;
10374 cartesian_trajectory_planner_B.b_i < 36;
10375 cartesian_trajectory_planner_B.b_i++) {
10376 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.b_i]
10377 -= cartesian_trajectory_planner_B.s_i *
10378 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.b_i];
10379 }
10380
10381 guard1 = true;
10382 } else {
10383 cartesian_trajectory_planner_B.b_i = grad->size[0];
10384 grad->size[0] = alpha->size[0];
10385 cartes_emxEnsureCapacity_real_T(grad,
10386 cartesian_trajectory_planner_B.b_i);
10387 cartesian_trajectory_planner_B.i_h = alpha->size[0];
10388 for (cartesian_trajectory_planner_B.b_i = 0;
10389 cartesian_trajectory_planner_B.b_i <
10390 cartesian_trajectory_planner_B.i_h;
10391 cartesian_trajectory_planner_B.b_i++) {
10392 grad->data[cartesian_trajectory_planner_B.b_i] = alpha->
10393 data[cartesian_trajectory_planner_B.b_i] - grad->
10394 data[cartesian_trajectory_planner_B.b_i];
10395 }
10396
10397 cartesian_trajectory_planner_B.b_gamma = 0.0;
10398 for (cartesian_trajectory_planner_B.b_i = 0;
10399 cartesian_trajectory_planner_B.b_i < 6;
10400 cartesian_trajectory_planner_B.b_i++) {
10401 cartesian_trajectory_planner_B.b_gamma +=
10402 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
10403 * grad->data[cartesian_trajectory_planner_B.b_i];
10404 }
10405
10406 cartesian_trajectory_planner_B.b_i = tmp->size[0] * tmp->size[1];
10407 tmp->size[0] = 1;
10408 tmp->size[1] = grad->size[0];
10409 cartes_emxEnsureCapacity_real_T(tmp,
10410 cartesian_trajectory_planner_B.b_i);
10411 cartesian_trajectory_planner_B.i_h = grad->size[0];
10412 for (cartesian_trajectory_planner_B.b_i = 0;
10413 cartesian_trajectory_planner_B.b_i <
10414 cartesian_trajectory_planner_B.i_h;
10415 cartesian_trajectory_planner_B.b_i++) {
10416 tmp->data[cartesian_trajectory_planner_B.b_i] = 0.2 * grad->
10417 data[cartesian_trajectory_planner_B.b_i];
10418 }
10419
10420 cartesian_trajectory_planner_B.s_i = 0.0;
10421 for (cartesian_trajectory_planner_B.b_i = 0;
10422 cartesian_trajectory_planner_B.b_i < 6;
10423 cartesian_trajectory_planner_B.b_i++) {
10424 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10425 = 0.0;
10426 for (cartesian_trajectory_planner_B.idx = 0;
10427 cartesian_trajectory_planner_B.idx < 6;
10428 cartesian_trajectory_planner_B.idx++) {
10429 cartesian_trajectory_planner_B.beta =
10430 cartesian_trajectory_planner_B.H[6 *
10431 cartesian_trajectory_planner_B.b_i +
10432 cartesian_trajectory_planner_B.idx] * tmp->
10433 data[cartesian_trajectory_planner_B.idx] +
10434 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i];
10435 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10436 = cartesian_trajectory_planner_B.beta;
10437 }
10438
10439 cartesian_trajectory_planner_B.s_i +=
10440 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10441 * grad->data[cartesian_trajectory_planner_B.b_i];
10442 }
10443
10444 if (cartesian_trajectory_planner_B.b_gamma <
10445 cartesian_trajectory_planner_B.s_i) {
10446 cartesian_trajectory_planner_B.b_i = tmp_0->size[0] *
10447 tmp_0->size[1];
10448 tmp_0->size[0] = 1;
10449 tmp_0->size[1] = grad->size[0];
10450 cartes_emxEnsureCapacity_real_T(tmp_0,
10451 cartesian_trajectory_planner_B.b_i);
10452 cartesian_trajectory_planner_B.i_h = grad->size[0];
10453 for (cartesian_trajectory_planner_B.b_i = 0;
10454 cartesian_trajectory_planner_B.b_i <
10455 cartesian_trajectory_planner_B.i_h;
10456 cartesian_trajectory_planner_B.b_i++) {
10457 tmp_0->data[cartesian_trajectory_planner_B.b_i] = 0.8 *
10458 grad->data[cartesian_trajectory_planner_B.b_i];
10459 }
10460
10461 cartesian_trajectory_planner_B.s_i = 0.0;
10462 for (cartesian_trajectory_planner_B.b_i = 0;
10463 cartesian_trajectory_planner_B.b_i < 6;
10464 cartesian_trajectory_planner_B.b_i++) {
10465 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10466 = 0.0;
10467 for (cartesian_trajectory_planner_B.idx = 0;
10468 cartesian_trajectory_planner_B.idx < 6;
10469 cartesian_trajectory_planner_B.idx++) {
10470 cartesian_trajectory_planner_B.beta =
10471 cartesian_trajectory_planner_B.H[6 *
10472 cartesian_trajectory_planner_B.b_i +
10473 cartesian_trajectory_planner_B.idx] * tmp_0->
10474 data[cartesian_trajectory_planner_B.idx] +
10475 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i];
10476 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10477 = cartesian_trajectory_planner_B.beta;
10478 }
10479
10480 cartesian_trajectory_planner_B.s_i +=
10481 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10482 * grad->data[cartesian_trajectory_planner_B.b_i];
10483 }
10484
10485 cartesian_trajectory_planner_B.b_i = grad_0->size[0] *
10486 grad_0->size[1];
10487 grad_0->size[0] = 1;
10488 grad_0->size[1] = grad->size[0];
10489 cartes_emxEnsureCapacity_real_T(grad_0,
10490 cartesian_trajectory_planner_B.b_i);
10491 cartesian_trajectory_planner_B.i_h = grad->size[0];
10492 for (cartesian_trajectory_planner_B.b_i = 0;
10493 cartesian_trajectory_planner_B.b_i <
10494 cartesian_trajectory_planner_B.i_h;
10495 cartesian_trajectory_planner_B.b_i++) {
10496 grad_0->data[cartesian_trajectory_planner_B.b_i] = grad->
10497 data[cartesian_trajectory_planner_B.b_i];
10498 }
10499
10500 cartesian_trajectory_planner_B.beta = 0.0;
10501 cartesian_trajectory_planner_B.b_gamma = 0.0;
10502 for (cartesian_trajectory_planner_B.b_i = 0;
10503 cartesian_trajectory_planner_B.b_i < 6;
10504 cartesian_trajectory_planner_B.b_i++) {
10505 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10506 = 0.0;
10507 for (cartesian_trajectory_planner_B.idx = 0;
10508 cartesian_trajectory_planner_B.idx < 6;
10509 cartesian_trajectory_planner_B.idx++) {
10510 cartesian_trajectory_planner_B.sigma =
10511 cartesian_trajectory_planner_B.H[6 *
10512 cartesian_trajectory_planner_B.b_i +
10513 cartesian_trajectory_planner_B.idx] * grad_0->
10514 data[cartesian_trajectory_planner_B.idx] +
10515 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i];
10516 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10517 = cartesian_trajectory_planner_B.sigma;
10518 }
10519
10520 cartesian_trajectory_planner_B.beta +=
10521 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10522 * grad->data[cartesian_trajectory_planner_B.b_i];
10523 cartesian_trajectory_planner_B.b_gamma +=
10524 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
10525 * grad->data[cartesian_trajectory_planner_B.b_i];
10526 }
10527
10528 cartesian_trajectory_planner_B.b_gamma =
10529 cartesian_trajectory_planner_B.s_i /
10530 (cartesian_trajectory_planner_B.beta -
10531 cartesian_trajectory_planner_B.b_gamma);
10532 } else {
10533 cartesian_trajectory_planner_B.b_gamma = 1.0;
10534 }
10535
10536 cartesian_trajectory_planner_B.beta = 0.0;
10537 for (cartesian_trajectory_planner_B.b_i = 0;
10538 cartesian_trajectory_planner_B.b_i < 6;
10539 cartesian_trajectory_planner_B.b_i++) {
10540 cartesian_trajectory_planner_B.s_i = 0.0;
10541 for (cartesian_trajectory_planner_B.idx = 0;
10542 cartesian_trajectory_planner_B.idx < 6;
10543 cartesian_trajectory_planner_B.idx++) {
10544 cartesian_trajectory_planner_B.s_i +=
10545 cartesian_trajectory_planner_B.H[6 *
10546 cartesian_trajectory_planner_B.idx +
10547 cartesian_trajectory_planner_B.b_i] * (1.0 -
10548 cartesian_trajectory_planner_B.b_gamma) * grad->
10549 data[cartesian_trajectory_planner_B.idx];
10550 }
10551
10552 cartesian_trajectory_planner_B.s_i +=
10553 cartesian_trajectory_planner_B.b_gamma *
10554 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
10555 cartesian_trajectory_planner_B.beta +=
10556 cartesian_trajectory_planner_B.s_i * grad->
10557 data[cartesian_trajectory_planner_B.b_i];
10558 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10559 = cartesian_trajectory_planner_B.s_i;
10560 }
10561
10562 cartesian_trajectory_planner_B.b_i = sNew->size[0] * sNew->size[1];
10563 sNew->size[0] = 6;
10564 sNew->size[1] = grad->size[0];
10565 cartes_emxEnsureCapacity_real_T(sNew,
10566 cartesian_trajectory_planner_B.b_i);
10567 cartesian_trajectory_planner_B.i_h = grad->size[0];
10568 for (cartesian_trajectory_planner_B.b_i = 0;
10569 cartesian_trajectory_planner_B.b_i <
10570 cartesian_trajectory_planner_B.i_h;
10571 cartesian_trajectory_planner_B.b_i++) {
10572 for (cartesian_trajectory_planner_B.idx = 0;
10573 cartesian_trajectory_planner_B.idx < 6;
10574 cartesian_trajectory_planner_B.idx++) {
10575 cartesian_trajectory_planner_B.s_i =
10576 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.idx]
10577 * grad->data[cartesian_trajectory_planner_B.b_i];
10578 sNew->data[cartesian_trajectory_planner_B.idx + 6 *
10579 cartesian_trajectory_planner_B.b_i] =
10580 cartesian_trajectory_planner_B.s_i /
10581 cartesian_trajectory_planner_B.beta;
10582 }
10583 }
10584
10585 for (cartesian_trajectory_planner_B.b_i = 0;
10586 cartesian_trajectory_planner_B.b_i < 36;
10587 cartesian_trajectory_planner_B.b_i++) {
10588 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.b_i]
10589 =
10590 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
10591 - sNew->data[cartesian_trajectory_planner_B.b_i];
10592 }
10593
10594 for (cartesian_trajectory_planner_B.b_i = 0;
10595 cartesian_trajectory_planner_B.b_i < 6;
10596 cartesian_trajectory_planner_B.b_i++) {
10597 for (cartesian_trajectory_planner_B.idx = 0;
10598 cartesian_trajectory_planner_B.idx < 6;
10599 cartesian_trajectory_planner_B.idx++) {
10600 cartesian_trajectory_planner_B.nx_j =
10601 cartesian_trajectory_planner_B.b_i + 6 *
10602 cartesian_trajectory_planner_B.idx;
10603 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.nx_j]
10604 = 0.0;
10605 for (cartesian_trajectory_planner_B.i_h = 0;
10606 cartesian_trajectory_planner_B.i_h < 6;
10607 cartesian_trajectory_planner_B.i_h++) {
10608 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.nx_j]
10609 += cartesian_trajectory_planner_B.V[6 *
10610 cartesian_trajectory_planner_B.i_h +
10611 cartesian_trajectory_planner_B.b_i] *
10612 cartesian_trajectory_planner_B.H[6 *
10613 cartesian_trajectory_planner_B.idx +
10614 cartesian_trajectory_planner_B.i_h];
10615 }
10616 }
10617 }
10618
10619 for (cartesian_trajectory_planner_B.b_i = 0;
10620 cartesian_trajectory_planner_B.b_i < 6;
10621 cartesian_trajectory_planner_B.b_i++) {
10622 for (cartesian_trajectory_planner_B.idx = 0;
10623 cartesian_trajectory_planner_B.idx < 6;
10624 cartesian_trajectory_planner_B.idx++) {
10625 cartesian_trajectory_planner_B.nx_j =
10626 cartesian_trajectory_planner_B.b_i + 6 *
10627 cartesian_trajectory_planner_B.idx;
10628 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.nx_j]
10629 = 0.0;
10630 for (cartesian_trajectory_planner_B.i_h = 0;
10631 cartesian_trajectory_planner_B.i_h < 6;
10632 cartesian_trajectory_planner_B.i_h++) {
10633 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.nx_j]
10634 += cartesian_trajectory_planner_B.H_m[6 *
10635 cartesian_trajectory_planner_B.i_h +
10636 cartesian_trajectory_planner_B.b_i] *
10637 cartesian_trajectory_planner_B.V[6 *
10638 cartesian_trajectory_planner_B.i_h +
10639 cartesian_trajectory_planner_B.idx];
10640 }
10641 }
10642 }
10643
10644 for (cartesian_trajectory_planner_B.b_i = 0;
10645 cartesian_trajectory_planner_B.b_i < 6;
10646 cartesian_trajectory_planner_B.b_i++) {
10647 for (cartesian_trajectory_planner_B.idx = 0;
10648 cartesian_trajectory_planner_B.idx < 6;
10649 cartesian_trajectory_planner_B.idx++) {
10650 cartesian_trajectory_planner_B.sNew[cartesian_trajectory_planner_B.idx
10651 + 6 * cartesian_trajectory_planner_B.b_i] =
10652 cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.idx]
10653 * cartesian_trajectory_planner_B.sNew_a[cartesian_trajectory_planner_B.b_i]
10654 / cartesian_trajectory_planner_B.beta;
10655 }
10656 }
10657
10658 for (cartesian_trajectory_planner_B.b_i = 0;
10659 cartesian_trajectory_planner_B.b_i < 36;
10660 cartesian_trajectory_planner_B.b_i++) {
10661 cartesian_trajectory_planner_B.s_i =
10662 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.b_i]
10663 + cartesian_trajectory_planner_B.sNew[cartesian_trajectory_planner_B.b_i];
10664 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.b_i]
10665 = 1.4901161193847656E-8 * static_cast<real_T>
10666 (tmp_1[cartesian_trajectory_planner_B.b_i]) +
10667 cartesian_trajectory_planner_B.s_i;
10668 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.b_i]
10669 = cartesian_trajectory_planner_B.s_i;
10670 }
10671
10672 if (!cartesian_tr_isPositiveDefinite
10673 (cartesian_trajectory_planner_B.H_m)) {
10674 *exitFlag = HessianNotPositiveSemidefinite;
10675 args = obj->ExtraArgs;
10676 for (cartesian_trajectory_planner_B.b_i = 0;
10677 cartesian_trajectory_planner_B.b_i < 36;
10678 cartesian_trajectory_planner_B.b_i++) {
10679 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
10680 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
10681 }
10682
10683 cartesian_trajectory_planner_B.b_i = grad->size[0];
10684 grad->size[0] = args->ErrTemp->size[0];
10685 cartes_emxEnsureCapacity_real_T(grad,
10686 cartesian_trajectory_planner_B.b_i);
10687 cartesian_trajectory_planner_B.i_h = args->ErrTemp->size[0];
10688 for (cartesian_trajectory_planner_B.b_i = 0;
10689 cartesian_trajectory_planner_B.b_i <
10690 cartesian_trajectory_planner_B.i_h;
10691 cartesian_trajectory_planner_B.b_i++) {
10692 grad->data[cartesian_trajectory_planner_B.b_i] = args->
10693 ErrTemp->data[cartesian_trajectory_planner_B.b_i];
10694 }
10695
10696 for (cartesian_trajectory_planner_B.b_i = 0;
10697 cartesian_trajectory_planner_B.b_i < 6;
10698 cartesian_trajectory_planner_B.b_i++) {
10699 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
10700 = 0.0;
10701 for (cartesian_trajectory_planner_B.idx = 0;
10702 cartesian_trajectory_planner_B.idx < 6;
10703 cartesian_trajectory_planner_B.idx++) {
10704 cartesian_trajectory_planner_B.A_d =
10705 cartesian_trajectory_planner_B.unusedU0[6 *
10706 cartesian_trajectory_planner_B.idx +
10707 cartesian_trajectory_planner_B.b_i] * grad->
10708 data[cartesian_trajectory_planner_B.idx] +
10709 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
10710 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
10711 = cartesian_trajectory_planner_B.A_d;
10712 }
10713 }
10714
10715 *err = cartesian_trajectory_pla_norm_a
10716 (cartesian_trajectory_planner_B.x);
10717 *iter = static_cast<real_T>
10718 (cartesian_trajectory_planner_B.g_idx_0) + 1.0;
10719 exitg2 = 1;
10720 } else {
10721 guard1 = true;
10722 }
10723 }
10724 }
10725
10726 if (guard1) {
10727 if (DampedBFGSwGradientProjectio_as(obj, xSol)) {
10728 for (cartesian_trajectory_planner_B.i_h = 0;
10729 cartesian_trajectory_planner_B.i_h < 6;
10730 cartesian_trajectory_planner_B.i_h++) {
10731 xSol[cartesian_trajectory_planner_B.i_h] =
10732 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_h];
10733 }
10734
10735 *exitFlag = SearchDirectionInvalid;
10736 args = obj->ExtraArgs;
10737 for (cartesian_trajectory_planner_B.b_i = 0;
10738 cartesian_trajectory_planner_B.b_i < 36;
10739 cartesian_trajectory_planner_B.b_i++) {
10740 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
10741 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
10742 }
10743
10744 cartesian_trajectory_planner_B.b_i = grad->size[0];
10745 grad->size[0] = args->ErrTemp->size[0];
10746 cartes_emxEnsureCapacity_real_T(grad,
10747 cartesian_trajectory_planner_B.b_i);
10748 cartesian_trajectory_planner_B.i_h = args->ErrTemp->size[0];
10749 for (cartesian_trajectory_planner_B.b_i = 0;
10750 cartesian_trajectory_planner_B.b_i <
10751 cartesian_trajectory_planner_B.i_h;
10752 cartesian_trajectory_planner_B.b_i++) {
10753 grad->data[cartesian_trajectory_planner_B.b_i] = args->
10754 ErrTemp->data[cartesian_trajectory_planner_B.b_i];
10755 }
10756
10757 for (cartesian_trajectory_planner_B.b_i = 0;
10758 cartesian_trajectory_planner_B.b_i < 6;
10759 cartesian_trajectory_planner_B.b_i++) {
10760 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
10761 = 0.0;
10762 for (cartesian_trajectory_planner_B.idx = 0;
10763 cartesian_trajectory_planner_B.idx < 6;
10764 cartesian_trajectory_planner_B.idx++) {
10765 cartesian_trajectory_planner_B.A_d =
10766 cartesian_trajectory_planner_B.unusedU0[6 *
10767 cartesian_trajectory_planner_B.idx +
10768 cartesian_trajectory_planner_B.b_i] * grad->
10769 data[cartesian_trajectory_planner_B.idx] +
10770 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
10771 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
10772 = cartesian_trajectory_planner_B.A_d;
10773 }
10774 }
10775
10776 *err = cartesian_trajectory_pla_norm_a
10777 (cartesian_trajectory_planner_B.x);
10778 *iter = static_cast<real_T>(cartesian_trajectory_planner_B.g_idx_0)
10779 + 1.0;
10780 exitg2 = 1;
10781 } else {
10782 for (cartesian_trajectory_planner_B.i_h = 0;
10783 cartesian_trajectory_planner_B.i_h < 6;
10784 cartesian_trajectory_planner_B.i_h++) {
10785 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_h]
10786 = xSol[cartesian_trajectory_planner_B.i_h];
10787 }
10788
10789 cartesian_trajectory_planner_B.b_i = grad->size[0];
10790 grad->size[0] = alpha->size[0];
10791 cartes_emxEnsureCapacity_real_T(grad,
10792 cartesian_trajectory_planner_B.b_i);
10793 cartesian_trajectory_planner_B.i_h = alpha->size[0];
10794 for (cartesian_trajectory_planner_B.b_i = 0;
10795 cartesian_trajectory_planner_B.b_i <
10796 cartesian_trajectory_planner_B.i_h;
10797 cartesian_trajectory_planner_B.b_i++) {
10798 grad->data[cartesian_trajectory_planner_B.b_i] = alpha->
10799 data[cartesian_trajectory_planner_B.b_i];
10800 }
10801
10802 cartesian_trajectory_planner_B.cost =
10803 cartesian_trajectory_planner_B.costNew;
10804 cartesian_trajectory_planner_B.g_idx_0++;
10805 }
10806 }
10807 }
10808 }
10809 } else {
10810 *exitFlag = IterationLimitExceeded;
10811 args = obj->ExtraArgs;
10812 for (cartesian_trajectory_planner_B.b_i = 0;
10813 cartesian_trajectory_planner_B.b_i < 36;
10814 cartesian_trajectory_planner_B.b_i++) {
10815 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
10816 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
10817 }
10818
10819 cartesian_trajectory_planner_B.b_i = grad->size[0];
10820 grad->size[0] = args->ErrTemp->size[0];
10821 cartes_emxEnsureCapacity_real_T(grad, cartesian_trajectory_planner_B.b_i);
10822 cartesian_trajectory_planner_B.i_h = args->ErrTemp->size[0];
10823 for (cartesian_trajectory_planner_B.b_i = 0;
10824 cartesian_trajectory_planner_B.b_i <
10825 cartesian_trajectory_planner_B.i_h;
10826 cartesian_trajectory_planner_B.b_i++) {
10827 grad->data[cartesian_trajectory_planner_B.b_i] = args->ErrTemp->
10828 data[cartesian_trajectory_planner_B.b_i];
10829 }
10830
10831 for (cartesian_trajectory_planner_B.b_i = 0;
10832 cartesian_trajectory_planner_B.b_i < 6;
10833 cartesian_trajectory_planner_B.b_i++) {
10834 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i] =
10835 0.0;
10836 for (cartesian_trajectory_planner_B.idx = 0;
10837 cartesian_trajectory_planner_B.idx < 6;
10838 cartesian_trajectory_planner_B.idx++) {
10839 cartesian_trajectory_planner_B.A_d =
10840 cartesian_trajectory_planner_B.unusedU0[6 *
10841 cartesian_trajectory_planner_B.idx +
10842 cartesian_trajectory_planner_B.b_i] * grad->
10843 data[cartesian_trajectory_planner_B.idx] +
10844 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
10845 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i] =
10846 cartesian_trajectory_planner_B.A_d;
10847 }
10848 }
10849
10850 *err = cartesian_trajectory_pla_norm_a(cartesian_trajectory_planner_B.x);
10851 *iter = obj->MaxNumIterationInternal;
10852 exitg2 = 1;
10853 }
10854 } while (exitg2 == 0);
10855
10856 cartesian_traje_emxFree_int32_T(&ii_3);
10857 cartesian_trajec_emxFree_real_T(&alpha_0);
10858 cartesian_trajec_emxFree_real_T(&A_3);
10859 cartesian_trajec_emxFree_real_T(&grad_1);
10860 cartesian_traje_emxFree_int32_T(&ii_2);
10861 cartesian_traje_emxFree_int32_T(&ii_1);
10862 cartesian_trajec_emxFree_real_T(&sNew);
10863 cartesian_trajec_emxFree_real_T(&grad_0);
10864 cartesian_trajec_emxFree_real_T(&tmp_0);
10865 cartesian_trajec_emxFree_real_T(&tmp);
10866 cartesian_trajec_emxFree_real_T(&sigma);
10867 cartesian_trajec_emxFree_real_T(&A_2);
10868 cartesian_tra_emxFree_boolean_T(&x);
10869 cartesian_trajec_emxFree_real_T(&y_0);
10870 cartesian_traje_emxFree_int32_T(&ii_0);
10871 cartesian_trajec_emxFree_real_T(&y);
10872 cartesian_traje_emxFree_int32_T(&ii);
10873 cartesian_trajec_emxFree_real_T(&a);
10874 cartesian_traje_emxFree_int32_T(&gb);
10875 cartesian_traje_emxFree_int32_T(&fb);
10876 cartesian_traje_emxFree_int32_T(&eb);
10877 cartesian_traje_emxFree_int32_T(&db);
10878 cartesian_traje_emxFree_int32_T(&cb);
10879 cartesian_trajec_emxFree_real_T(&L);
10880 cartesian_trajec_emxFree_real_T(&AIn);
10881 cartesian_trajec_emxFree_real_T(&alpha);
10882 cartesian_trajec_emxFree_real_T(&A);
10883 cartesian_tra_emxFree_boolean_T(&activeSet);
10884 cartesian_trajec_emxFree_real_T(&grad);
10885 cartesian_trajec_emxFree_real_T(&unusedU1);
10886}
10887
10888static void cartesian_trajectory_p_isfinite(const
10889 emxArray_real_T_cartesian_tra_T *x, emxArray_boolean_T_cartesian__T *b)
10890{
10891 emxArray_boolean_T_cartesian__T *tmp;
10892 int32_T loop_ub;
10893 int32_T i;
10894 i = b->size[0];
10895 b->size[0] = x->size[0];
10896 car_emxEnsureCapacity_boolean_T(b, i);
10897 loop_ub = x->size[0];
10898 for (i = 0; i < loop_ub; i++) {
10899 b->data[i] = rtIsInf(x->data[i]);
10900 }
10901
10902 cartesian_tra_emxInit_boolean_T(&tmp, 1);
10903 i = tmp->size[0];
10904 tmp->size[0] = x->size[0];
10905 car_emxEnsureCapacity_boolean_T(tmp, i);
10906 loop_ub = x->size[0];
10907 for (i = 0; i < loop_ub; i++) {
10908 tmp->data[i] = rtIsNaN(x->data[i]);
10909 }
10910
10911 i = b->size[0];
10912 car_emxEnsureCapacity_boolean_T(b, i);
10913 loop_ub = b->size[0];
10914 for (i = 0; i < loop_ub; i++) {
10915 b->data[i] = ((!b->data[i]) && (!tmp->data[i]));
10916 }
10917
10918 cartesian_tra_emxFree_boolean_T(&tmp);
10919}
10920
10921static void cartesi_genrand_uint32_vector_a(uint32_T mt[625], uint32_T u[2])
10922{
10923 for (cartesian_trajectory_planner_B.b_j_b = 0;
10924 cartesian_trajectory_planner_B.b_j_b < 2;
10925 cartesian_trajectory_planner_B.b_j_b++) {
10926 cartesian_trajectory_planner_B.mti = mt[624] + 1U;
10927 if (cartesian_trajectory_planner_B.mti >= 625U) {
10928 for (cartesian_trajectory_planner_B.b_kk = 0;
10929 cartesian_trajectory_planner_B.b_kk < 227;
10930 cartesian_trajectory_planner_B.b_kk++) {
10931 cartesian_trajectory_planner_B.y_m =
10932 (mt[cartesian_trajectory_planner_B.b_kk + 1] & 2147483647U) |
10933 (mt[cartesian_trajectory_planner_B.b_kk] & 2147483648U);
10934 if ((cartesian_trajectory_planner_B.y_m & 1U) == 0U) {
10935 cartesian_trajectory_planner_B.y_m >>= 1U;
10936 } else {
10937 cartesian_trajectory_planner_B.y_m =
10938 cartesian_trajectory_planner_B.y_m >> 1U ^ 2567483615U;
10939 }
10940
10941 mt[cartesian_trajectory_planner_B.b_kk] =
10942 mt[cartesian_trajectory_planner_B.b_kk + 397] ^
10943 cartesian_trajectory_planner_B.y_m;
10944 }
10945
10946 for (cartesian_trajectory_planner_B.b_kk = 0;
10947 cartesian_trajectory_planner_B.b_kk < 396;
10948 cartesian_trajectory_planner_B.b_kk++) {
10949 cartesian_trajectory_planner_B.y_m =
10950 (mt[cartesian_trajectory_planner_B.b_kk + 227] & 2147483648U) |
10951 (mt[cartesian_trajectory_planner_B.b_kk + 228] & 2147483647U);
10952 if ((cartesian_trajectory_planner_B.y_m & 1U) == 0U) {
10953 cartesian_trajectory_planner_B.y_m >>= 1U;
10954 } else {
10955 cartesian_trajectory_planner_B.y_m =
10956 cartesian_trajectory_planner_B.y_m >> 1U ^ 2567483615U;
10957 }
10958
10959 mt[cartesian_trajectory_planner_B.b_kk + 227] =
10960 mt[cartesian_trajectory_planner_B.b_kk] ^
10961 cartesian_trajectory_planner_B.y_m;
10962 }
10963
10964 cartesian_trajectory_planner_B.y_m = (mt[623] & 2147483648U) | (mt[0] &
10965 2147483647U);
10966 if ((cartesian_trajectory_planner_B.y_m & 1U) == 0U) {
10967 cartesian_trajectory_planner_B.y_m >>= 1U;
10968 } else {
10969 cartesian_trajectory_planner_B.y_m = cartesian_trajectory_planner_B.y_m >>
10970 1U ^ 2567483615U;
10971 }
10972
10973 mt[623] = mt[396] ^ cartesian_trajectory_planner_B.y_m;
10974 cartesian_trajectory_planner_B.mti = 1U;
10975 }
10976
10977 cartesian_trajectory_planner_B.y_m = mt[static_cast<int32_T>
10978 (cartesian_trajectory_planner_B.mti) - 1];
10979 mt[624] = cartesian_trajectory_planner_B.mti;
10980 cartesian_trajectory_planner_B.y_m ^= cartesian_trajectory_planner_B.y_m >>
10981 11U;
10982 cartesian_trajectory_planner_B.y_m ^= cartesian_trajectory_planner_B.y_m <<
10983 7U & 2636928640U;
10984 cartesian_trajectory_planner_B.y_m ^= cartesian_trajectory_planner_B.y_m <<
10985 15U & 4022730752U;
10986 u[cartesian_trajectory_planner_B.b_j_b] = cartesian_trajectory_planner_B.y_m
10987 >> 18U ^ cartesian_trajectory_planner_B.y_m;
10988 }
10989}
10990
10991static boolean_T cartesian_trajec_is_valid_state(const uint32_T mt[625])
10992{
10993 boolean_T isvalid;
10994 boolean_T exitg1;
10995 if ((mt[624] >= 1U) && (mt[624] < 625U)) {
10996 isvalid = true;
10997 } else {
10998 isvalid = false;
10999 }
11000
11001 if (isvalid) {
11002 isvalid = false;
11003 cartesian_trajectory_planner_B.k_o = 0;
11004 exitg1 = false;
11005 while ((!exitg1) && (cartesian_trajectory_planner_B.k_o + 1 < 625)) {
11006 if (mt[cartesian_trajectory_planner_B.k_o] == 0U) {
11007 cartesian_trajectory_planner_B.k_o++;
11008 } else {
11009 isvalid = true;
11010 exitg1 = true;
11011 }
11012 }
11013 }
11014
11015 return isvalid;
11016}
11017
11018static real_T cartesian_trajectory_genrandu_a(uint32_T mt[625])
11019{
11020 real_T r;
11021 int32_T exitg1;
11022
11023 // ========================= COPYRIGHT NOTICE ============================
11024 // This is a uniform (0,1) pseudorandom number generator based on:
11025 //
11026 // A C-program for MT19937, with initialization improved 2002/1/26.
11027 // Coded by Takuji Nishimura and Makoto Matsumoto.
11028 //
11029 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
11030 // All rights reserved.
11031 //
11032 // Redistribution and use in source and binary forms, with or without
11033 // modification, are permitted provided that the following conditions
11034 // are met:
11035 //
11036 // 1. Redistributions of source code must retain the above copyright
11037 // notice, this list of conditions and the following disclaimer.
11038 //
11039 // 2. Redistributions in binary form must reproduce the above copyright
11040 // notice, this list of conditions and the following disclaimer
11041 // in the documentation and/or other materials provided with the
11042 // distribution.
11043 //
11044 // 3. The names of its contributors may not be used to endorse or
11045 // promote products derived from this software without specific
11046 // prior written permission.
11047 //
11048 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
11049 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
11050 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
11051 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
11052 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
11053 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
11054 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
11055 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
11056 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
11057 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
11058 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
11059 //
11060 // ============================= END =================================
11061 do {
11062 exitg1 = 0;
11063 cartesi_genrand_uint32_vector_a(mt, cartesian_trajectory_planner_B.b_u_i);
11064 r = (static_cast<real_T>(cartesian_trajectory_planner_B.b_u_i[0] >> 5U) *
11065 6.7108864E+7 + static_cast<real_T>
11066 (cartesian_trajectory_planner_B.b_u_i[1] >> 6U)) *
11067 1.1102230246251565E-16;
11068 if (r == 0.0) {
11069 if (!cartesian_trajec_is_valid_state(mt)) {
11070 cartesian_trajectory_planner_B.r_g = 5489U;
11071 mt[0] = 5489U;
11072 for (cartesian_trajectory_planner_B.b_mti_m = 0;
11073 cartesian_trajectory_planner_B.b_mti_m < 623;
11074 cartesian_trajectory_planner_B.b_mti_m++) {
11075 cartesian_trajectory_planner_B.r_g =
11076 ((cartesian_trajectory_planner_B.r_g >> 30U ^
11077 cartesian_trajectory_planner_B.r_g) * 1812433253U +
11078 cartesian_trajectory_planner_B.b_mti_m) + 1U;
11079 mt[cartesian_trajectory_planner_B.b_mti_m + 1] =
11080 cartesian_trajectory_planner_B.r_g;
11081 }
11082
11083 mt[624] = 624U;
11084 }
11085 } else {
11086 exitg1 = 1;
11087 }
11088 } while (exitg1 == 0);
11089
11090 return r;
11091}
11092
11093static real_T cartesia_eml_rand_mt19937ar_ast(uint32_T state[625])
11094{
11095 real_T r;
11096 static const real_T tmp[257] = { 1.0, 0.977101701267673, 0.959879091800108,
11097 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
11098 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
11099 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
11100 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
11101 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
11102 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
11103 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
11104 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
11105 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
11106 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
11107 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
11108 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
11109 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
11110 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
11111 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
11112 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
11113 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
11114 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
11115 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
11116 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
11117 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
11118 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
11119 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
11120 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
11121 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
11122 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
11123 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
11124 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
11125 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
11126 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
11127 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
11128 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
11129 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
11130 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
11131 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
11132 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
11133 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
11134 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
11135 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
11136 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
11137 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
11138 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
11139 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
11140 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
11141 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
11142 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
11143 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
11144 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
11145 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
11146 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
11147 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
11148 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
11149 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
11150 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
11151 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
11152 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
11153 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
11154 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
11155 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
11156 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
11157 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
11158 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
11159 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
11160 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
11161 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
11162 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
11163 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
11164 0.000477467764609386 };
11165
11166 const real_T *fitab;
11167 int32_T exitg1;
11168 cartesian_trajectory_planner_B.xi[0] = 0.0;
11169 cartesian_trajectory_planner_B.xi[1] = 0.215241895984875;
11170 cartesian_trajectory_planner_B.xi[2] = 0.286174591792068;
11171 cartesian_trajectory_planner_B.xi[3] = 0.335737519214422;
11172 cartesian_trajectory_planner_B.xi[4] = 0.375121332878378;
11173 cartesian_trajectory_planner_B.xi[5] = 0.408389134611989;
11174 cartesian_trajectory_planner_B.xi[6] = 0.43751840220787;
11175 cartesian_trajectory_planner_B.xi[7] = 0.46363433679088;
11176 cartesian_trajectory_planner_B.xi[8] = 0.487443966139235;
11177 cartesian_trajectory_planner_B.xi[9] = 0.50942332960209;
11178 cartesian_trajectory_planner_B.xi[10] = 0.529909720661557;
11179 cartesian_trajectory_planner_B.xi[11] = 0.549151702327164;
11180 cartesian_trajectory_planner_B.xi[12] = 0.567338257053817;
11181 cartesian_trajectory_planner_B.xi[13] = 0.584616766106378;
11182 cartesian_trajectory_planner_B.xi[14] = 0.601104617755991;
11183 cartesian_trajectory_planner_B.xi[15] = 0.61689699000775;
11184 cartesian_trajectory_planner_B.xi[16] = 0.63207223638606;
11185 cartesian_trajectory_planner_B.xi[17] = 0.646695714894993;
11186 cartesian_trajectory_planner_B.xi[18] = 0.660822574244419;
11187 cartesian_trajectory_planner_B.xi[19] = 0.674499822837293;
11188 cartesian_trajectory_planner_B.xi[20] = 0.687767892795788;
11189 cartesian_trajectory_planner_B.xi[21] = 0.700661841106814;
11190 cartesian_trajectory_planner_B.xi[22] = 0.713212285190975;
11191 cartesian_trajectory_planner_B.xi[23] = 0.725446140909999;
11192 cartesian_trajectory_planner_B.xi[24] = 0.737387211434295;
11193 cartesian_trajectory_planner_B.xi[25] = 0.749056662017815;
11194 cartesian_trajectory_planner_B.xi[26] = 0.760473406430107;
11195 cartesian_trajectory_planner_B.xi[27] = 0.771654424224568;
11196 cartesian_trajectory_planner_B.xi[28] = 0.782615023307232;
11197 cartesian_trajectory_planner_B.xi[29] = 0.793369058840623;
11198 cartesian_trajectory_planner_B.xi[30] = 0.80392911698997;
11199 cartesian_trajectory_planner_B.xi[31] = 0.814306670135215;
11200 cartesian_trajectory_planner_B.xi[32] = 0.824512208752291;
11201 cartesian_trajectory_planner_B.xi[33] = 0.834555354086381;
11202 cartesian_trajectory_planner_B.xi[34] = 0.844444954909153;
11203 cartesian_trajectory_planner_B.xi[35] = 0.854189171008163;
11204 cartesian_trajectory_planner_B.xi[36] = 0.863795545553308;
11205 cartesian_trajectory_planner_B.xi[37] = 0.87327106808886;
11206 cartesian_trajectory_planner_B.xi[38] = 0.882622229585165;
11207 cartesian_trajectory_planner_B.xi[39] = 0.891855070732941;
11208 cartesian_trajectory_planner_B.xi[40] = 0.900975224461221;
11209 cartesian_trajectory_planner_B.xi[41] = 0.909987953496718;
11210 cartesian_trajectory_planner_B.xi[42] = 0.91889818364959;
11211 cartesian_trajectory_planner_B.xi[43] = 0.927710533401999;
11212 cartesian_trajectory_planner_B.xi[44] = 0.936429340286575;
11213 cartesian_trajectory_planner_B.xi[45] = 0.945058684468165;
11214 cartesian_trajectory_planner_B.xi[46] = 0.953602409881086;
11215 cartesian_trajectory_planner_B.xi[47] = 0.96206414322304;
11216 cartesian_trajectory_planner_B.xi[48] = 0.970447311064224;
11217 cartesian_trajectory_planner_B.xi[49] = 0.978755155294224;
11218 cartesian_trajectory_planner_B.xi[50] = 0.986990747099062;
11219 cartesian_trajectory_planner_B.xi[51] = 0.99515699963509;
11220 cartesian_trajectory_planner_B.xi[52] = 1.00325667954467;
11221 cartesian_trajectory_planner_B.xi[53] = 1.01129241744;
11222 cartesian_trajectory_planner_B.xi[54] = 1.01926671746548;
11223 cartesian_trajectory_planner_B.xi[55] = 1.02718196603564;
11224 cartesian_trajectory_planner_B.xi[56] = 1.03504043983344;
11225 cartesian_trajectory_planner_B.xi[57] = 1.04284431314415;
11226 cartesian_trajectory_planner_B.xi[58] = 1.05059566459093;
11227 cartesian_trajectory_planner_B.xi[59] = 1.05829648333067;
11228 cartesian_trajectory_planner_B.xi[60] = 1.06594867476212;
11229 cartesian_trajectory_planner_B.xi[61] = 1.07355406579244;
11230 cartesian_trajectory_planner_B.xi[62] = 1.0811144097034;
11231 cartesian_trajectory_planner_B.xi[63] = 1.08863139065398;
11232 cartesian_trajectory_planner_B.xi[64] = 1.09610662785202;
11233 cartesian_trajectory_planner_B.xi[65] = 1.10354167942464;
11234 cartesian_trajectory_planner_B.xi[66] = 1.11093804601357;
11235 cartesian_trajectory_planner_B.xi[67] = 1.11829717411934;
11236 cartesian_trajectory_planner_B.xi[68] = 1.12562045921553;
11237 cartesian_trajectory_planner_B.xi[69] = 1.13290924865253;
11238 cartesian_trajectory_planner_B.xi[70] = 1.14016484436815;
11239 cartesian_trajectory_planner_B.xi[71] = 1.14738850542085;
11240 cartesian_trajectory_planner_B.xi[72] = 1.15458145035993;
11241 cartesian_trajectory_planner_B.xi[73] = 1.16174485944561;
11242 cartesian_trajectory_planner_B.xi[74] = 1.16887987673083;
11243 cartesian_trajectory_planner_B.xi[75] = 1.17598761201545;
11244 cartesian_trajectory_planner_B.xi[76] = 1.18306914268269;
11245 cartesian_trajectory_planner_B.xi[77] = 1.19012551542669;
11246 cartesian_trajectory_planner_B.xi[78] = 1.19715774787944;
11247 cartesian_trajectory_planner_B.xi[79] = 1.20416683014438;
11248 cartesian_trajectory_planner_B.xi[80] = 1.2111537262437;
11249 cartesian_trajectory_planner_B.xi[81] = 1.21811937548548;
11250 cartesian_trajectory_planner_B.xi[82] = 1.22506469375653;
11251 cartesian_trajectory_planner_B.xi[83] = 1.23199057474614;
11252 cartesian_trajectory_planner_B.xi[84] = 1.23889789110569;
11253 cartesian_trajectory_planner_B.xi[85] = 1.24578749554863;
11254 cartesian_trajectory_planner_B.xi[86] = 1.2526602218949;
11255 cartesian_trajectory_planner_B.xi[87] = 1.25951688606371;
11256 cartesian_trajectory_planner_B.xi[88] = 1.26635828701823;
11257 cartesian_trajectory_planner_B.xi[89] = 1.27318520766536;
11258 cartesian_trajectory_planner_B.xi[90] = 1.27999841571382;
11259 cartesian_trajectory_planner_B.xi[91] = 1.28679866449324;
11260 cartesian_trajectory_planner_B.xi[92] = 1.29358669373695;
11261 cartesian_trajectory_planner_B.xi[93] = 1.30036323033084;
11262 cartesian_trajectory_planner_B.xi[94] = 1.30712898903073;
11263 cartesian_trajectory_planner_B.xi[95] = 1.31388467315022;
11264 cartesian_trajectory_planner_B.xi[96] = 1.32063097522106;
11265 cartesian_trajectory_planner_B.xi[97] = 1.32736857762793;
11266 cartesian_trajectory_planner_B.xi[98] = 1.33409815321936;
11267 cartesian_trajectory_planner_B.xi[99] = 1.3408203658964;
11268 cartesian_trajectory_planner_B.xi[100] = 1.34753587118059;
11269 cartesian_trajectory_planner_B.xi[101] = 1.35424531676263;
11270 cartesian_trajectory_planner_B.xi[102] = 1.36094934303328;
11271 cartesian_trajectory_planner_B.xi[103] = 1.36764858359748;
11272 cartesian_trajectory_planner_B.xi[104] = 1.37434366577317;
11273 cartesian_trajectory_planner_B.xi[105] = 1.38103521107586;
11274 cartesian_trajectory_planner_B.xi[106] = 1.38772383568998;
11275 cartesian_trajectory_planner_B.xi[107] = 1.39441015092814;
11276 cartesian_trajectory_planner_B.xi[108] = 1.40109476367925;
11277 cartesian_trajectory_planner_B.xi[109] = 1.4077782768464;
11278 cartesian_trajectory_planner_B.xi[110] = 1.41446128977547;
11279 cartesian_trajectory_planner_B.xi[111] = 1.42114439867531;
11280 cartesian_trajectory_planner_B.xi[112] = 1.42782819703026;
11281 cartesian_trajectory_planner_B.xi[113] = 1.43451327600589;
11282 cartesian_trajectory_planner_B.xi[114] = 1.44120022484872;
11283 cartesian_trajectory_planner_B.xi[115] = 1.44788963128058;
11284 cartesian_trajectory_planner_B.xi[116] = 1.45458208188841;
11285 cartesian_trajectory_planner_B.xi[117] = 1.46127816251028;
11286 cartesian_trajectory_planner_B.xi[118] = 1.46797845861808;
11287 cartesian_trajectory_planner_B.xi[119] = 1.47468355569786;
11288 cartesian_trajectory_planner_B.xi[120] = 1.48139403962819;
11289 cartesian_trajectory_planner_B.xi[121] = 1.48811049705745;
11290 cartesian_trajectory_planner_B.xi[122] = 1.49483351578049;
11291 cartesian_trajectory_planner_B.xi[123] = 1.50156368511546;
11292 cartesian_trajectory_planner_B.xi[124] = 1.50830159628131;
11293 cartesian_trajectory_planner_B.xi[125] = 1.51504784277671;
11294 cartesian_trajectory_planner_B.xi[126] = 1.521803020761;
11295 cartesian_trajectory_planner_B.xi[127] = 1.52856772943771;
11296 cartesian_trajectory_planner_B.xi[128] = 1.53534257144151;
11297 cartesian_trajectory_planner_B.xi[129] = 1.542128153229;
11298 cartesian_trajectory_planner_B.xi[130] = 1.54892508547417;
11299 cartesian_trajectory_planner_B.xi[131] = 1.55573398346918;
11300 cartesian_trajectory_planner_B.xi[132] = 1.56255546753104;
11301 cartesian_trajectory_planner_B.xi[133] = 1.56939016341512;
11302 cartesian_trajectory_planner_B.xi[134] = 1.57623870273591;
11303 cartesian_trajectory_planner_B.xi[135] = 1.58310172339603;
11304 cartesian_trajectory_planner_B.xi[136] = 1.58997987002419;
11305 cartesian_trajectory_planner_B.xi[137] = 1.59687379442279;
11306 cartesian_trajectory_planner_B.xi[138] = 1.60378415602609;
11307 cartesian_trajectory_planner_B.xi[139] = 1.61071162236983;
11308 cartesian_trajectory_planner_B.xi[140] = 1.61765686957301;
11309 cartesian_trajectory_planner_B.xi[141] = 1.62462058283303;
11310 cartesian_trajectory_planner_B.xi[142] = 1.63160345693487;
11311 cartesian_trajectory_planner_B.xi[143] = 1.63860619677555;
11312 cartesian_trajectory_planner_B.xi[144] = 1.64562951790478;
11313 cartesian_trajectory_planner_B.xi[145] = 1.65267414708306;
11314 cartesian_trajectory_planner_B.xi[146] = 1.65974082285818;
11315 cartesian_trajectory_planner_B.xi[147] = 1.66683029616166;
11316 cartesian_trajectory_planner_B.xi[148] = 1.67394333092612;
11317 cartesian_trajectory_planner_B.xi[149] = 1.68108070472517;
11318 cartesian_trajectory_planner_B.xi[150] = 1.68824320943719;
11319 cartesian_trajectory_planner_B.xi[151] = 1.69543165193456;
11320 cartesian_trajectory_planner_B.xi[152] = 1.70264685479992;
11321 cartesian_trajectory_planner_B.xi[153] = 1.7098896570713;
11322 cartesian_trajectory_planner_B.xi[154] = 1.71716091501782;
11323 cartesian_trajectory_planner_B.xi[155] = 1.72446150294804;
11324 cartesian_trajectory_planner_B.xi[156] = 1.73179231405296;
11325 cartesian_trajectory_planner_B.xi[157] = 1.73915426128591;
11326 cartesian_trajectory_planner_B.xi[158] = 1.74654827828172;
11327 cartesian_trajectory_planner_B.xi[159] = 1.75397532031767;
11328 cartesian_trajectory_planner_B.xi[160] = 1.76143636531891;
11329 cartesian_trajectory_planner_B.xi[161] = 1.76893241491127;
11330 cartesian_trajectory_planner_B.xi[162] = 1.77646449552452;
11331 cartesian_trajectory_planner_B.xi[163] = 1.78403365954944;
11332 cartesian_trajectory_planner_B.xi[164] = 1.79164098655216;
11333 cartesian_trajectory_planner_B.xi[165] = 1.79928758454972;
11334 cartesian_trajectory_planner_B.xi[166] = 1.80697459135082;
11335 cartesian_trajectory_planner_B.xi[167] = 1.81470317596628;
11336 cartesian_trajectory_planner_B.xi[168] = 1.82247454009388;
11337 cartesian_trajectory_planner_B.xi[169] = 1.83028991968276;
11338 cartesian_trajectory_planner_B.xi[170] = 1.83815058658281;
11339 cartesian_trajectory_planner_B.xi[171] = 1.84605785028518;
11340 cartesian_trajectory_planner_B.xi[172] = 1.8540130597602;
11341 cartesian_trajectory_planner_B.xi[173] = 1.86201760539967;
11342 cartesian_trajectory_planner_B.xi[174] = 1.87007292107127;
11343 cartesian_trajectory_planner_B.xi[175] = 1.878180486293;
11344 cartesian_trajectory_planner_B.xi[176] = 1.88634182853678;
11345 cartesian_trajectory_planner_B.xi[177] = 1.8945585256707;
11346 cartesian_trajectory_planner_B.xi[178] = 1.90283220855043;
11347 cartesian_trajectory_planner_B.xi[179] = 1.91116456377125;
11348 cartesian_trajectory_planner_B.xi[180] = 1.91955733659319;
11349 cartesian_trajectory_planner_B.xi[181] = 1.92801233405266;
11350 cartesian_trajectory_planner_B.xi[182] = 1.93653142827569;
11351 cartesian_trajectory_planner_B.xi[183] = 1.94511656000868;
11352 cartesian_trajectory_planner_B.xi[184] = 1.95376974238465;
11353 cartesian_trajectory_planner_B.xi[185] = 1.96249306494436;
11354 cartesian_trajectory_planner_B.xi[186] = 1.97128869793366;
11355 cartesian_trajectory_planner_B.xi[187] = 1.98015889690048;
11356 cartesian_trajectory_planner_B.xi[188] = 1.98910600761744;
11357 cartesian_trajectory_planner_B.xi[189] = 1.99813247135842;
11358 cartesian_trajectory_planner_B.xi[190] = 2.00724083056053;
11359 cartesian_trajectory_planner_B.xi[191] = 2.0164337349062;
11360 cartesian_trajectory_planner_B.xi[192] = 2.02571394786385;
11361 cartesian_trajectory_planner_B.xi[193] = 2.03508435372962;
11362 cartesian_trajectory_planner_B.xi[194] = 2.04454796521753;
11363 cartesian_trajectory_planner_B.xi[195] = 2.05410793165065;
11364 cartesian_trajectory_planner_B.xi[196] = 2.06376754781173;
11365 cartesian_trajectory_planner_B.xi[197] = 2.07353026351874;
11366 cartesian_trajectory_planner_B.xi[198] = 2.0833996939983;
11367 cartesian_trajectory_planner_B.xi[199] = 2.09337963113879;
11368 cartesian_trajectory_planner_B.xi[200] = 2.10347405571488;
11369 cartesian_trajectory_planner_B.xi[201] = 2.11368715068665;
11370 cartesian_trajectory_planner_B.xi[202] = 2.12402331568952;
11371 cartesian_trajectory_planner_B.xi[203] = 2.13448718284602;
11372 cartesian_trajectory_planner_B.xi[204] = 2.14508363404789;
11373 cartesian_trajectory_planner_B.xi[205] = 2.15581781987674;
11374 cartesian_trajectory_planner_B.xi[206] = 2.16669518035431;
11375 cartesian_trajectory_planner_B.xi[207] = 2.17772146774029;
11376 cartesian_trajectory_planner_B.xi[208] = 2.18890277162636;
11377 cartesian_trajectory_planner_B.xi[209] = 2.20024554661128;
11378 cartesian_trajectory_planner_B.xi[210] = 2.21175664288416;
11379 cartesian_trajectory_planner_B.xi[211] = 2.22344334009251;
11380 cartesian_trajectory_planner_B.xi[212] = 2.23531338492992;
11381 cartesian_trajectory_planner_B.xi[213] = 2.24737503294739;
11382 cartesian_trajectory_planner_B.xi[214] = 2.25963709517379;
11383 cartesian_trajectory_planner_B.xi[215] = 2.27210899022838;
11384 cartesian_trajectory_planner_B.xi[216] = 2.28480080272449;
11385 cartesian_trajectory_planner_B.xi[217] = 2.29772334890286;
11386 cartesian_trajectory_planner_B.xi[218] = 2.31088825060137;
11387 cartesian_trajectory_planner_B.xi[219] = 2.32430801887113;
11388 cartesian_trajectory_planner_B.xi[220] = 2.33799614879653;
11389 cartesian_trajectory_planner_B.xi[221] = 2.35196722737914;
11390 cartesian_trajectory_planner_B.xi[222] = 2.36623705671729;
11391 cartesian_trajectory_planner_B.xi[223] = 2.38082279517208;
11392 cartesian_trajectory_planner_B.xi[224] = 2.39574311978193;
11393 cartesian_trajectory_planner_B.xi[225] = 2.41101841390112;
11394 cartesian_trajectory_planner_B.xi[226] = 2.42667098493715;
11395 cartesian_trajectory_planner_B.xi[227] = 2.44272531820036;
11396 cartesian_trajectory_planner_B.xi[228] = 2.4592083743347;
11397 cartesian_trajectory_planner_B.xi[229] = 2.47614993967052;
11398 cartesian_trajectory_planner_B.xi[230] = 2.49358304127105;
11399 cartesian_trajectory_planner_B.xi[231] = 2.51154444162669;
11400 cartesian_trajectory_planner_B.xi[232] = 2.53007523215985;
11401 cartesian_trajectory_planner_B.xi[233] = 2.54922155032478;
11402 cartesian_trajectory_planner_B.xi[234] = 2.56903545268184;
11403 cartesian_trajectory_planner_B.xi[235] = 2.58957598670829;
11404 cartesian_trajectory_planner_B.xi[236] = 2.61091051848882;
11405 cartesian_trajectory_planner_B.xi[237] = 2.63311639363158;
11406 cartesian_trajectory_planner_B.xi[238] = 2.65628303757674;
11407 cartesian_trajectory_planner_B.xi[239] = 2.68051464328574;
11408 cartesian_trajectory_planner_B.xi[240] = 2.70593365612306;
11409 cartesian_trajectory_planner_B.xi[241] = 2.73268535904401;
11410 cartesian_trajectory_planner_B.xi[242] = 2.76094400527999;
11411 cartesian_trajectory_planner_B.xi[243] = 2.79092117400193;
11412 cartesian_trajectory_planner_B.xi[244] = 2.82287739682644;
11413 cartesian_trajectory_planner_B.xi[245] = 2.85713873087322;
11414 cartesian_trajectory_planner_B.xi[246] = 2.89412105361341;
11415 cartesian_trajectory_planner_B.xi[247] = 2.93436686720889;
11416 cartesian_trajectory_planner_B.xi[248] = 2.97860327988184;
11417 cartesian_trajectory_planner_B.xi[249] = 3.02783779176959;
11418 cartesian_trajectory_planner_B.xi[250] = 3.08352613200214;
11419 cartesian_trajectory_planner_B.xi[251] = 3.147889289518;
11420 cartesian_trajectory_planner_B.xi[252] = 3.2245750520478;
11421 cartesian_trajectory_planner_B.xi[253] = 3.32024473383983;
11422 cartesian_trajectory_planner_B.xi[254] = 3.44927829856143;
11423 cartesian_trajectory_planner_B.xi[255] = 3.65415288536101;
11424 cartesian_trajectory_planner_B.xi[256] = 3.91075795952492;
11425 fitab = &tmp[0];
11426 do {
11427 exitg1 = 0;
11428 cartesi_genrand_uint32_vector_a(state, cartesian_trajectory_planner_B.u32);
11429 cartesian_trajectory_planner_B.i_e = static_cast<int32_T>
11430 ((cartesian_trajectory_planner_B.u32[1] >> 24U) + 1U);
11431 r = ((static_cast<real_T>(cartesian_trajectory_planner_B.u32[0] >> 3U) *
11432 1.6777216E+7 + static_cast<real_T>(static_cast<int32_T>
11433 (cartesian_trajectory_planner_B.u32[1]) & 16777215)) *
11434 2.2204460492503131E-16 - 1.0) *
11435 cartesian_trajectory_planner_B.xi[cartesian_trajectory_planner_B.i_e];
11436 if (fabs(r) <=
11437 cartesian_trajectory_planner_B.xi[cartesian_trajectory_planner_B.i_e - 1])
11438 {
11439 exitg1 = 1;
11440 } else if (cartesian_trajectory_planner_B.i_e < 256) {
11441 cartesian_trajectory_planner_B.x_c = cartesian_trajectory_genrandu_a(state);
11442 if ((fitab[cartesian_trajectory_planner_B.i_e - 1] -
11443 fitab[cartesian_trajectory_planner_B.i_e]) *
11444 cartesian_trajectory_planner_B.x_c +
11445 fitab[cartesian_trajectory_planner_B.i_e] < exp(-0.5 * r * r)) {
11446 exitg1 = 1;
11447 }
11448 } else {
11449 do {
11450 cartesian_trajectory_planner_B.x_c = cartesian_trajectory_genrandu_a
11451 (state);
11452 cartesian_trajectory_planner_B.x_c = log
11453 (cartesian_trajectory_planner_B.x_c) * 0.273661237329758;
11454 cartesian_trajectory_planner_B.d_u = cartesian_trajectory_genrandu_a
11455 (state);
11456 } while (!(-2.0 * log(cartesian_trajectory_planner_B.d_u) >
11457 cartesian_trajectory_planner_B.x_c *
11458 cartesian_trajectory_planner_B.x_c));
11459
11460 if (r < 0.0) {
11461 r = cartesian_trajectory_planner_B.x_c - 3.65415288536101;
11462 } else {
11463 r = 3.65415288536101 - cartesian_trajectory_planner_B.x_c;
11464 }
11465
11466 exitg1 = 1;
11467 }
11468 } while (exitg1 == 0);
11469
11470 return r;
11471}
11472
11473static void cartesian_trajectory_plan_randn(const real_T varargin_1[2],
11474 emxArray_real_T_cartesian_tra_T *r)
11475{
11476 cartesian_trajectory_planner_B.b_k_c = r->size[0] * r->size[1];
11477 r->size[0] = static_cast<int32_T>(varargin_1[0]);
11478 r->size[1] = 1;
11479 cartes_emxEnsureCapacity_real_T(r, cartesian_trajectory_planner_B.b_k_c);
11480 cartesian_trajectory_planner_B.d_j = r->size[0] - 1;
11481 for (cartesian_trajectory_planner_B.b_k_c = 0;
11482 cartesian_trajectory_planner_B.b_k_c <=
11483 cartesian_trajectory_planner_B.d_j; cartesian_trajectory_planner_B.b_k_c
11484 ++) {
11485 r->data[cartesian_trajectory_planner_B.b_k_c] =
11486 cartesia_eml_rand_mt19937ar_ast(cartesian_trajectory_planner_DW.state_m);
11487 }
11488}
11489
11490static void cartesian__eml_rand_mt19937ar_a(const uint32_T state[625], uint32_T
11491 b_state[625], real_T *r)
11492{
11493 int32_T exitg1;
11494 memcpy(&b_state[0], &state[0], 625U * sizeof(uint32_T));
11495
11496 // ========================= COPYRIGHT NOTICE ============================
11497 // This is a uniform (0,1) pseudorandom number generator based on:
11498 //
11499 // A C-program for MT19937, with initialization improved 2002/1/26.
11500 // Coded by Takuji Nishimura and Makoto Matsumoto.
11501 //
11502 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
11503 // All rights reserved.
11504 //
11505 // Redistribution and use in source and binary forms, with or without
11506 // modification, are permitted provided that the following conditions
11507 // are met:
11508 //
11509 // 1. Redistributions of source code must retain the above copyright
11510 // notice, this list of conditions and the following disclaimer.
11511 //
11512 // 2. Redistributions in binary form must reproduce the above copyright
11513 // notice, this list of conditions and the following disclaimer
11514 // in the documentation and/or other materials provided with the
11515 // distribution.
11516 //
11517 // 3. The names of its contributors may not be used to endorse or
11518 // promote products derived from this software without specific
11519 // prior written permission.
11520 //
11521 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
11522 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
11523 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
11524 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
11525 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
11526 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
11527 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
11528 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
11529 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
11530 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
11531 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
11532 //
11533 // ============================= END =================================
11534 do {
11535 exitg1 = 0;
11536 cartesi_genrand_uint32_vector_a(b_state, cartesian_trajectory_planner_B.b_u);
11537 *r = (static_cast<real_T>(cartesian_trajectory_planner_B.b_u[0] >> 5U) *
11538 6.7108864E+7 + static_cast<real_T>(cartesian_trajectory_planner_B.b_u
11539 [1] >> 6U)) * 1.1102230246251565E-16;
11540 if (*r == 0.0) {
11541 if (!cartesian_trajec_is_valid_state(b_state)) {
11542 cartesian_trajectory_planner_B.r = 5489U;
11543 b_state[0] = 5489U;
11544 for (cartesian_trajectory_planner_B.b_mti = 0;
11545 cartesian_trajectory_planner_B.b_mti < 623;
11546 cartesian_trajectory_planner_B.b_mti++) {
11547 cartesian_trajectory_planner_B.r = ((cartesian_trajectory_planner_B.r >>
11548 30U ^ cartesian_trajectory_planner_B.r) * 1812433253U +
11549 cartesian_trajectory_planner_B.b_mti) + 1U;
11550 b_state[cartesian_trajectory_planner_B.b_mti + 1] =
11551 cartesian_trajectory_planner_B.r;
11552 }
11553
11554 b_state[624] = 624U;
11555 }
11556 } else {
11557 exitg1 = 1;
11558 }
11559 } while (exitg1 == 0);
11560}
11561
11562static void cartesian_trajectory_pla_rand_a(real_T varargin_1,
11563 emxArray_real_T_cartesian_tra_T *r)
11564{
11565 cartesian_trajectory_planner_B.b_k_m = r->size[0];
11566 cartesian_trajectory_planner_B.d_d = static_cast<int32_T>(varargin_1);
11567 r->size[0] = cartesian_trajectory_planner_B.d_d;
11568 cartes_emxEnsureCapacity_real_T(r, cartesian_trajectory_planner_B.b_k_m);
11569 cartesian_trajectory_planner_B.d_d--;
11570 for (cartesian_trajectory_planner_B.b_k_m = 0;
11571 cartesian_trajectory_planner_B.b_k_m <=
11572 cartesian_trajectory_planner_B.d_d; cartesian_trajectory_planner_B.b_k_m
11573 ++) {
11574 memcpy(&cartesian_trajectory_planner_B.uv1[0],
11575 &cartesian_trajectory_planner_DW.state_m[0], 625U * sizeof(uint32_T));
11576 cartesian__eml_rand_mt19937ar_a(cartesian_trajectory_planner_B.uv1,
11577 cartesian_trajectory_planner_DW.state_m, &r->
11578 data[cartesian_trajectory_planner_B.b_k_m]);
11579 }
11580}
11581
11582static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
11583 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
11584 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
11585 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
11586 solutionInfo_Status_size[2])
11587{
11588 emxArray_real_T_cartesian_tra_T *newseed;
11589 f_robotics_manip_internal_IKE_T *args;
11590 x_robotics_manip_internal_Rig_T *obj_0;
11591 emxArray_real_T_cartesian_tra_T *qi;
11592 c_rigidBodyJoint_cartesian_as_T *obj_1;
11593 emxArray_real_T_cartesian_tra_T *ub;
11594 emxArray_real_T_cartesian_tra_T *lb;
11595 emxArray_real_T_cartesian_tra_T *rn;
11596 emxArray_real_T_cartesian_tra_T *e;
11597 emxArray_boolean_T_cartesian__T *x;
11598 emxArray_boolean_T_cartesian__T *x_tmp;
11599 emxArray_boolean_T_cartesian__T *x_tmp_0;
11600 emxArray_boolean_T_cartesian__T *x_0;
11601 static const char_T tmp[14] = { 'b', 'e', 's', 't', ' ', 'a', 'v', 'a', 'i',
11602 'l', 'a', 'b', 'l', 'e' };
11603
11604 static const char_T tmp_0[7] = { 's', 'u', 'c', 'c', 'e', 's', 's' };
11605
11606 boolean_T guard1 = false;
11607 boolean_T guard2 = false;
11608 boolean_T guard3 = false;
11609 boolean_T exitg1;
11610 boolean_T exitg2;
11611 obj->MaxNumIterationInternal = obj->MaxNumIteration;
11612 obj->MaxTimeInternal = obj->MaxTime;
11613 for (cartesian_trajectory_planner_B.i_f = 0;
11614 cartesian_trajectory_planner_B.i_f < 6;
11615 cartesian_trajectory_planner_B.i_f++) {
11616 obj->SeedInternal[cartesian_trajectory_planner_B.i_f] =
11617 seed[cartesian_trajectory_planner_B.i_f];
11618 }
11619
11620 cartesian_trajectory_planner_B.tol = obj->SolutionTolerance;
11621 obj->TimeObj.StartTime = ctimefun();
11622 DampedBFGSwGradientProjection_s(obj, xSol,
11623 &cartesian_trajectory_planner_B.exitFlag,
11624 &cartesian_trajectory_planner_B.err, &cartesian_trajectory_planner_B.iter);
11625 *solutionInfo_RRAttempts = 0.0;
11626 *solutionInfo_Iterations = cartesian_trajectory_planner_B.iter;
11627 *solutionInfo_Error = cartesian_trajectory_planner_B.err;
11628 cartesian_trajectory_planner_B.exitFlagPrev =
11629 cartesian_trajectory_planner_B.exitFlag;
11630 cartesian_trajec_emxInit_real_T(&newseed, 1);
11631 cartesian_trajec_emxInit_real_T(&qi, 2);
11632 cartesian_trajec_emxInit_real_T(&ub, 1);
11633 cartesian_trajec_emxInit_real_T(&lb, 1);
11634 cartesian_trajec_emxInit_real_T(&rn, 1);
11635 cartesian_trajec_emxInit_real_T(&e, 2);
11636 cartesian_tra_emxInit_boolean_T(&x, 1);
11637 cartesian_tra_emxInit_boolean_T(&x_tmp, 1);
11638 cartesian_tra_emxInit_boolean_T(&x_tmp_0, 1);
11639 cartesian_tra_emxInit_boolean_T(&x_0, 1);
11640 exitg1 = false;
11641 while ((!exitg1) && (obj->RandomRestart && (cartesian_trajectory_planner_B.err
11642 > cartesian_trajectory_planner_B.tol))) {
11643 obj->MaxNumIterationInternal -= cartesian_trajectory_planner_B.iter;
11644 cartesian_trajectory_planner_B.err = ctimefun();
11645 cartesian_trajectory_planner_B.err -= obj->TimeObj.StartTime;
11646 obj->MaxTimeInternal = obj->MaxTime - cartesian_trajectory_planner_B.err;
11647 if (obj->MaxNumIterationInternal <= 0.0) {
11648 cartesian_trajectory_planner_B.exitFlag = IterationLimitExceeded;
11649 }
11650
11651 if ((cartesian_trajectory_planner_B.exitFlag == IterationLimitExceeded) ||
11652 (cartesian_trajectory_planner_B.exitFlag == TimeLimitExceeded)) {
11653 cartesian_trajectory_planner_B.exitFlagPrev =
11654 cartesian_trajectory_planner_B.exitFlag;
11655 exitg1 = true;
11656 } else {
11657 args = obj->ExtraArgs;
11658 obj_0 = args->Robot;
11659 cartesian_trajectory_planner_B.ix = newseed->size[0];
11660 newseed->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
11661 cartes_emxEnsureCapacity_real_T(newseed, cartesian_trajectory_planner_B.ix);
11662 cartesian_trajectory_planner_B.nx = static_cast<int32_T>
11663 (obj_0->PositionNumber);
11664 for (cartesian_trajectory_planner_B.ix = 0;
11665 cartesian_trajectory_planner_B.ix < cartesian_trajectory_planner_B.nx;
11666 cartesian_trajectory_planner_B.ix++) {
11667 newseed->data[cartesian_trajectory_planner_B.ix] = 0.0;
11668 }
11669
11670 cartesian_trajectory_planner_B.err = obj_0->NumBodies;
11671 cartesian_trajectory_planner_B.c_a = static_cast<int32_T>
11672 (cartesian_trajectory_planner_B.err) - 1;
11673 for (cartesian_trajectory_planner_B.i_f = 0;
11674 cartesian_trajectory_planner_B.i_f <=
11675 cartesian_trajectory_planner_B.c_a;
11676 cartesian_trajectory_planner_B.i_f++) {
11677 cartesian_trajectory_planner_B.err = obj_0->
11678 PositionDoFMap[cartesian_trajectory_planner_B.i_f];
11679 cartesian_trajectory_planner_B.iter = obj_0->
11680 PositionDoFMap[cartesian_trajectory_planner_B.i_f + 8];
11681 if (cartesian_trajectory_planner_B.err <=
11682 cartesian_trajectory_planner_B.iter) {
11683 obj_1 = obj_0->Bodies[cartesian_trajectory_planner_B.i_f]
11684 ->JointInternal;
11685 if (static_cast<int32_T>(obj_1->PositionNumber) == 0) {
11686 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
11687 qi->size[0] = 1;
11688 qi->size[1] = 1;
11689 cartes_emxEnsureCapacity_real_T(qi,
11690 cartesian_trajectory_planner_B.ix);
11691 qi->data[0] = (rtNaN);
11692 } else {
11693 cartesian_trajectory_planner_B.nx = obj_1->
11694 PositionLimitsInternal->size[0];
11695 cartesian_trajectory_planner_B.ix = ub->size[0];
11696 ub->size[0] = cartesian_trajectory_planner_B.nx;
11697 cartes_emxEnsureCapacity_real_T(ub,
11698 cartesian_trajectory_planner_B.ix);
11699 for (cartesian_trajectory_planner_B.ix = 0;
11700 cartesian_trajectory_planner_B.ix <
11701 cartesian_trajectory_planner_B.nx;
11702 cartesian_trajectory_planner_B.ix++) {
11703 ub->data[cartesian_trajectory_planner_B.ix] =
11704 obj_1->PositionLimitsInternal->
11705 data[cartesian_trajectory_planner_B.ix +
11706 obj_1->PositionLimitsInternal->size[0]];
11707 }
11708
11709 cartesian_trajectory_planner_B.nx = obj_1->
11710 PositionLimitsInternal->size[0];
11711 cartesian_trajectory_planner_B.ix = lb->size[0];
11712 lb->size[0] = cartesian_trajectory_planner_B.nx;
11713 cartes_emxEnsureCapacity_real_T(lb,
11714 cartesian_trajectory_planner_B.ix);
11715 for (cartesian_trajectory_planner_B.ix = 0;
11716 cartesian_trajectory_planner_B.ix <
11717 cartesian_trajectory_planner_B.nx;
11718 cartesian_trajectory_planner_B.ix++) {
11719 lb->data[cartesian_trajectory_planner_B.ix] =
11720 obj_1->PositionLimitsInternal->
11721 data[cartesian_trajectory_planner_B.ix];
11722 }
11723
11724 cartesian_trajectory_p_isfinite(lb, x_tmp);
11725 cartesian_trajectory_planner_B.y_n = true;
11726 cartesian_trajectory_planner_B.ix = 0;
11727 exitg2 = false;
11728 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
11729 x_tmp->size[0])) {
11730 if (!x_tmp->data[cartesian_trajectory_planner_B.ix]) {
11731 cartesian_trajectory_planner_B.y_n = false;
11732 exitg2 = true;
11733 } else {
11734 cartesian_trajectory_planner_B.ix++;
11735 }
11736 }
11737
11738 guard1 = false;
11739 guard2 = false;
11740 guard3 = false;
11741 if (cartesian_trajectory_planner_B.y_n) {
11742 cartesian_trajectory_p_isfinite(ub, x);
11743 cartesian_trajectory_planner_B.y_n = true;
11744 cartesian_trajectory_planner_B.ix = 0;
11745 exitg2 = false;
11746 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
11747 x->size[0])) {
11748 if (!x->data[cartesian_trajectory_planner_B.ix]) {
11749 cartesian_trajectory_planner_B.y_n = false;
11750 exitg2 = true;
11751 } else {
11752 cartesian_trajectory_planner_B.ix++;
11753 }
11754 }
11755
11756 if (cartesian_trajectory_planner_B.y_n) {
11757 cartesian_trajectory_pla_rand_a(obj_1->PositionNumber, rn);
11758 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
11759 qi->size[0] = lb->size[0];
11760 qi->size[1] = 1;
11761 cartes_emxEnsureCapacity_real_T(qi,
11762 cartesian_trajectory_planner_B.ix);
11763 cartesian_trajectory_planner_B.nx = lb->size[0] - 1;
11764 for (cartesian_trajectory_planner_B.ix = 0;
11765 cartesian_trajectory_planner_B.ix <=
11766 cartesian_trajectory_planner_B.nx;
11767 cartesian_trajectory_planner_B.ix++) {
11768 qi->data[cartesian_trajectory_planner_B.ix] = (ub->
11769 data[cartesian_trajectory_planner_B.ix] - lb->
11770 data[cartesian_trajectory_planner_B.ix]) * rn->
11771 data[cartesian_trajectory_planner_B.ix] + lb->
11772 data[cartesian_trajectory_planner_B.ix];
11773 }
11774 } else {
11775 guard3 = true;
11776 }
11777 } else {
11778 guard3 = true;
11779 }
11780
11781 if (guard3) {
11782 cartesian_trajectory_planner_B.y_n = true;
11783 cartesian_trajectory_planner_B.ix = 0;
11784 exitg2 = false;
11785 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
11786 x_tmp->size[0])) {
11787 if (!x_tmp->data[cartesian_trajectory_planner_B.ix]) {
11788 cartesian_trajectory_planner_B.y_n = false;
11789 exitg2 = true;
11790 } else {
11791 cartesian_trajectory_planner_B.ix++;
11792 }
11793 }
11794
11795 if (cartesian_trajectory_planner_B.y_n) {
11796 cartesian_trajectory_p_isfinite(ub, x);
11797 cartesian_trajectory_planner_B.ix = x_0->size[0];
11798 x_0->size[0] = x->size[0];
11799 car_emxEnsureCapacity_boolean_T(x_0,
11800 cartesian_trajectory_planner_B.ix);
11801 cartesian_trajectory_planner_B.nx = x->size[0];
11802 for (cartesian_trajectory_planner_B.ix = 0;
11803 cartesian_trajectory_planner_B.ix <
11804 cartesian_trajectory_planner_B.nx;
11805 cartesian_trajectory_planner_B.ix++) {
11806 x_0->data[cartesian_trajectory_planner_B.ix] = !x->
11807 data[cartesian_trajectory_planner_B.ix];
11808 }
11809
11810 if (cartesian_trajectory_planne_any(x_0)) {
11811 cartesian_trajectory_planner_B.ub[0] = lb->size[0];
11812 cartesian_trajectory_planner_B.ub[1] = 1.0;
11813 cartesian_trajectory_plan_randn
11814 (cartesian_trajectory_planner_B.ub, qi);
11815 cartesian_trajectory_planner_B.nx = qi->size[0] - 1;
11816 cartesian_trajectory_planner_B.ix = e->size[0] * e->size[1];
11817 e->size[0] = qi->size[0];
11818 e->size[1] = 1;
11819 cartes_emxEnsureCapacity_real_T(e,
11820 cartesian_trajectory_planner_B.ix);
11821 for (cartesian_trajectory_planner_B.ix = 0;
11822 cartesian_trajectory_planner_B.ix <=
11823 cartesian_trajectory_planner_B.nx;
11824 cartesian_trajectory_planner_B.ix++) {
11825 e->data[cartesian_trajectory_planner_B.ix] = fabs(qi->
11826 data[cartesian_trajectory_planner_B.ix]);
11827 }
11828
11829 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
11830 qi->size[0] = lb->size[0];
11831 qi->size[1] = 1;
11832 cartes_emxEnsureCapacity_real_T(qi,
11833 cartesian_trajectory_planner_B.ix);
11834 cartesian_trajectory_planner_B.nx = lb->size[0] - 1;
11835 for (cartesian_trajectory_planner_B.ix = 0;
11836 cartesian_trajectory_planner_B.ix <=
11837 cartesian_trajectory_planner_B.nx;
11838 cartesian_trajectory_planner_B.ix++) {
11839 qi->data[cartesian_trajectory_planner_B.ix] = lb->
11840 data[cartesian_trajectory_planner_B.ix] + e->
11841 data[cartesian_trajectory_planner_B.ix];
11842 }
11843 } else {
11844 guard2 = true;
11845 }
11846 } else {
11847 guard2 = true;
11848 }
11849 }
11850
11851 if (guard2) {
11852 cartesian_trajectory_planner_B.ix = x_tmp_0->size[0];
11853 x_tmp_0->size[0] = x_tmp->size[0];
11854 car_emxEnsureCapacity_boolean_T(x_tmp_0,
11855 cartesian_trajectory_planner_B.ix);
11856 cartesian_trajectory_planner_B.nx = x_tmp->size[0];
11857 for (cartesian_trajectory_planner_B.ix = 0;
11858 cartesian_trajectory_planner_B.ix <
11859 cartesian_trajectory_planner_B.nx;
11860 cartesian_trajectory_planner_B.ix++) {
11861 x_tmp_0->data[cartesian_trajectory_planner_B.ix] = !x_tmp->
11862 data[cartesian_trajectory_planner_B.ix];
11863 }
11864
11865 if (cartesian_trajectory_planne_any(x_tmp_0)) {
11866 cartesian_trajectory_p_isfinite(ub, x);
11867 cartesian_trajectory_planner_B.y_n = true;
11868 cartesian_trajectory_planner_B.ix = 0;
11869 exitg2 = false;
11870 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
11871 x->size[0])) {
11872 if (!x->data[cartesian_trajectory_planner_B.ix]) {
11873 cartesian_trajectory_planner_B.y_n = false;
11874 exitg2 = true;
11875 } else {
11876 cartesian_trajectory_planner_B.ix++;
11877 }
11878 }
11879
11880 if (cartesian_trajectory_planner_B.y_n) {
11881 cartesian_trajectory_planner_B.ub[0] = ub->size[0];
11882 cartesian_trajectory_planner_B.ub[1] = 1.0;
11883 cartesian_trajectory_plan_randn
11884 (cartesian_trajectory_planner_B.ub, qi);
11885 cartesian_trajectory_planner_B.nx = qi->size[0] - 1;
11886 cartesian_trajectory_planner_B.ix = e->size[0] * e->size[1];
11887 e->size[0] = qi->size[0];
11888 e->size[1] = 1;
11889 cartes_emxEnsureCapacity_real_T(e,
11890 cartesian_trajectory_planner_B.ix);
11891 for (cartesian_trajectory_planner_B.ix = 0;
11892 cartesian_trajectory_planner_B.ix <=
11893 cartesian_trajectory_planner_B.nx;
11894 cartesian_trajectory_planner_B.ix++) {
11895 e->data[cartesian_trajectory_planner_B.ix] = fabs(qi->
11896 data[cartesian_trajectory_planner_B.ix]);
11897 }
11898
11899 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
11900 qi->size[0] = ub->size[0];
11901 qi->size[1] = 1;
11902 cartes_emxEnsureCapacity_real_T(qi,
11903 cartesian_trajectory_planner_B.ix);
11904 cartesian_trajectory_planner_B.nx = ub->size[0] - 1;
11905 for (cartesian_trajectory_planner_B.ix = 0;
11906 cartesian_trajectory_planner_B.ix <=
11907 cartesian_trajectory_planner_B.nx;
11908 cartesian_trajectory_planner_B.ix++) {
11909 qi->data[cartesian_trajectory_planner_B.ix] = ub->
11910 data[cartesian_trajectory_planner_B.ix] - e->
11911 data[cartesian_trajectory_planner_B.ix];
11912 }
11913 } else {
11914 guard1 = true;
11915 }
11916 } else {
11917 guard1 = true;
11918 }
11919 }
11920
11921 if (guard1) {
11922 cartesian_trajectory_planner_B.ub[0] = ub->size[0];
11923 cartesian_trajectory_planner_B.ub[1] = 1.0;
11924 cartesian_trajectory_plan_randn(cartesian_trajectory_planner_B.ub,
11925 qi);
11926 }
11927 }
11928
11929 if (cartesian_trajectory_planner_B.err >
11930 cartesian_trajectory_planner_B.iter) {
11931 cartesian_trajectory_planner_B.nx = 0;
11932 cartesian_trajectory_planner_B.ix = 0;
11933 } else {
11934 cartesian_trajectory_planner_B.nx = static_cast<int32_T>
11935 (cartesian_trajectory_planner_B.err) - 1;
11936 cartesian_trajectory_planner_B.ix = static_cast<int32_T>
11937 (cartesian_trajectory_planner_B.iter);
11938 }
11939
11940 cartesian_trajectory_planner_B.unnamed_idx_1 =
11941 cartesian_trajectory_planner_B.ix -
11942 cartesian_trajectory_planner_B.nx;
11943 for (cartesian_trajectory_planner_B.ix = 0;
11944 cartesian_trajectory_planner_B.ix <
11945 cartesian_trajectory_planner_B.unnamed_idx_1;
11946 cartesian_trajectory_planner_B.ix++) {
11947 newseed->data[cartesian_trajectory_planner_B.nx +
11948 cartesian_trajectory_planner_B.ix] = qi->
11949 data[cartesian_trajectory_planner_B.ix];
11950 }
11951 }
11952 }
11953
11954 for (cartesian_trajectory_planner_B.ix = 0;
11955 cartesian_trajectory_planner_B.ix < 6;
11956 cartesian_trajectory_planner_B.ix++) {
11957 obj->SeedInternal[cartesian_trajectory_planner_B.ix] = newseed->
11958 data[cartesian_trajectory_planner_B.ix];
11959 }
11960
11961 DampedBFGSwGradientProjection_s(obj, cartesian_trajectory_planner_B.c_xSol,
11962 &cartesian_trajectory_planner_B.exitFlag,
11963 &cartesian_trajectory_planner_B.err,
11964 &cartesian_trajectory_planner_B.iter);
11965 if (cartesian_trajectory_planner_B.err < *solutionInfo_Error) {
11966 for (cartesian_trajectory_planner_B.i_f = 0;
11967 cartesian_trajectory_planner_B.i_f < 6;
11968 cartesian_trajectory_planner_B.i_f++) {
11969 xSol[cartesian_trajectory_planner_B.i_f] =
11970 cartesian_trajectory_planner_B.c_xSol[cartesian_trajectory_planner_B.i_f];
11971 }
11972
11973 *solutionInfo_Error = cartesian_trajectory_planner_B.err;
11974 cartesian_trajectory_planner_B.exitFlagPrev =
11975 cartesian_trajectory_planner_B.exitFlag;
11976 }
11977
11978 (*solutionInfo_RRAttempts)++;
11979 *solutionInfo_Iterations += cartesian_trajectory_planner_B.iter;
11980 }
11981 }
11982
11983 cartesian_tra_emxFree_boolean_T(&x_0);
11984 cartesian_tra_emxFree_boolean_T(&x_tmp_0);
11985 cartesian_tra_emxFree_boolean_T(&x_tmp);
11986 cartesian_tra_emxFree_boolean_T(&x);
11987 cartesian_trajec_emxFree_real_T(&e);
11988 cartesian_trajec_emxFree_real_T(&rn);
11989 cartesian_trajec_emxFree_real_T(&lb);
11990 cartesian_trajec_emxFree_real_T(&ub);
11991 cartesian_trajec_emxFree_real_T(&qi);
11992 cartesian_trajec_emxFree_real_T(&newseed);
11993 *solutionInfo_ExitFlag = cartesian_trajectory_planner_B.exitFlagPrev;
11994 if (*solutionInfo_Error < cartesian_trajectory_planner_B.tol) {
11995 solutionInfo_Status_size[0] = 1;
11996 solutionInfo_Status_size[1] = 7;
11997 for (cartesian_trajectory_planner_B.ix = 0;
11998 cartesian_trajectory_planner_B.ix < 7;
11999 cartesian_trajectory_planner_B.ix++) {
12000 solutionInfo_Status_data[cartesian_trajectory_planner_B.ix] =
12001 tmp_0[cartesian_trajectory_planner_B.ix];
12002 }
12003 } else {
12004 solutionInfo_Status_size[0] = 1;
12005 solutionInfo_Status_size[1] = 14;
12006 for (cartesian_trajectory_planner_B.ix = 0;
12007 cartesian_trajectory_planner_B.ix < 14;
12008 cartesian_trajectory_planner_B.ix++) {
12009 solutionInfo_Status_data[cartesian_trajectory_planner_B.ix] =
12010 tmp[cartesian_trajectory_planner_B.ix];
12011 }
12012 }
12013}
12014
12015static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
12016 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
12017 real_T QSol[6])
12018{
12019 emxArray_real_T_cartesian_tra_T *bodyIndices;
12020 emxArray_real_T_cartesian_tra_T *positionIndices;
12021 x_robotics_manip_internal_Rig_T *obj_0;
12022 emxArray_char_T_cartesian_tra_T *endEffectorName;
12023 w_robotics_manip_internal_Rig_T *body;
12024 emxArray_real_T_cartesian_tra_T *positionMap;
12025 emxArray_real_T_cartesian_tra_T *e;
12026 emxArray_int32_T_cartesian_tr_T *h;
12027 emxArray_real_T_cartesian_tra_T *y;
12028 emxArray_char_T_cartesian_tra_T *bname;
12029 emxArray_real_T_cartesian_tra_T *bodyIndices_0;
12030 boolean_T exitg1;
12031 c_inverseKinematics_setPoseGoal(obj, tform, weights);
12032 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <
12033 6; cartesian_trajectory_planner_B.i++) {
12034 QSol[cartesian_trajectory_planner_B.i] =
12035 initialGuess[cartesian_trajectory_planner_B.i];
12036 }
12037
12038 cartesian_traj_emxInit_char_T_a(&endEffectorName, 2);
12039 RigidBodyTree_validateConfigu_a(obj->RigidBodyTreeInternal, QSol);
12040 cartes_NLPSolverInterface_solve(obj->Solver, QSol,
12041 cartesian_trajectory_planner_B.qvSolRaw, &cartesian_trajectory_planner_B.bid,
12042 &cartesian_trajectory_planner_B.numPositions,
12043 &cartesian_trajectory_planner_B.ndbl, &cartesian_trajectory_planner_B.apnd,
12044 cartesian_trajectory_planner_B.sol_Status_data,
12045 cartesian_trajectory_planner_B.sol_Status_size);
12046 obj_0 = obj->RigidBodyTreeInternal;
12047 cartesian_trajectory_planner_B.partialTrueCount = endEffectorName->size[0] *
12048 endEffectorName->size[1];
12049 endEffectorName->size[0] = 1;
12050 endEffectorName->size[1] = obj->Solver->ExtraArgs->BodyName->size[1];
12051 cart_emxEnsureCapacity_char_T_a(endEffectorName,
12052 cartesian_trajectory_planner_B.partialTrueCount);
12053 cartesian_trajectory_planner_B.loop_ub = obj->Solver->ExtraArgs->
12054 BodyName->size[0] * obj->Solver->ExtraArgs->BodyName->size[1] - 1;
12055 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12056 cartesian_trajectory_planner_B.partialTrueCount <=
12057 cartesian_trajectory_planner_B.loop_ub;
12058 cartesian_trajectory_planner_B.partialTrueCount++) {
12059 endEffectorName->data[cartesian_trajectory_planner_B.partialTrueCount] =
12060 obj->Solver->ExtraArgs->BodyName->
12061 data[cartesian_trajectory_planner_B.partialTrueCount];
12062 }
12063
12064 cartesian_trajec_emxInit_real_T(&bodyIndices, 1);
12065 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices->size[0];
12066 bodyIndices->size[0] = static_cast<int32_T>(obj_0->NumBodies);
12067 cartes_emxEnsureCapacity_real_T(bodyIndices,
12068 cartesian_trajectory_planner_B.partialTrueCount);
12069 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>(obj_0->NumBodies);
12070 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12071 cartesian_trajectory_planner_B.partialTrueCount <
12072 cartesian_trajectory_planner_B.loop_ub;
12073 cartesian_trajectory_planner_B.partialTrueCount++) {
12074 bodyIndices->data[cartesian_trajectory_planner_B.partialTrueCount] = 0.0;
12075 }
12076
12077 cartesian_traj_emxInit_char_T_a(&bname, 2);
12078 cartesian_trajectory_planner_B.bid = -1.0;
12079 cartesian_trajectory_planner_B.partialTrueCount = bname->size[0] * bname->
12080 size[1];
12081 bname->size[0] = 1;
12082 bname->size[1] = obj_0->Base.NameInternal->size[1];
12083 cart_emxEnsureCapacity_char_T_a(bname,
12084 cartesian_trajectory_planner_B.partialTrueCount);
12085 cartesian_trajectory_planner_B.loop_ub = obj_0->Base.NameInternal->size[0] *
12086 obj_0->Base.NameInternal->size[1] - 1;
12087 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12088 cartesian_trajectory_planner_B.partialTrueCount <=
12089 cartesian_trajectory_planner_B.loop_ub;
12090 cartesian_trajectory_planner_B.partialTrueCount++) {
12091 bname->data[cartesian_trajectory_planner_B.partialTrueCount] =
12092 obj_0->Base.NameInternal->
12093 data[cartesian_trajectory_planner_B.partialTrueCount];
12094 }
12095
12096 if (cartesian_trajectory_pla_strcmp(bname, endEffectorName)) {
12097 cartesian_trajectory_planner_B.bid = 0.0;
12098 } else {
12099 cartesian_trajectory_planner_B.numPositions = obj_0->NumBodies;
12100 cartesian_trajectory_planner_B.i = 0;
12101 exitg1 = false;
12102 while ((!exitg1) && (cartesian_trajectory_planner_B.i <= static_cast<int32_T>
12103 (cartesian_trajectory_planner_B.numPositions) - 1)) {
12104 body = obj_0->Bodies[cartesian_trajectory_planner_B.i];
12105 cartesian_trajectory_planner_B.partialTrueCount = bname->size[0] *
12106 bname->size[1];
12107 bname->size[0] = 1;
12108 bname->size[1] = body->NameInternal->size[1];
12109 cart_emxEnsureCapacity_char_T_a(bname,
12110 cartesian_trajectory_planner_B.partialTrueCount);
12111 cartesian_trajectory_planner_B.loop_ub = body->NameInternal->size[0] *
12112 body->NameInternal->size[1] - 1;
12113 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12114 cartesian_trajectory_planner_B.partialTrueCount <=
12115 cartesian_trajectory_planner_B.loop_ub;
12116 cartesian_trajectory_planner_B.partialTrueCount++) {
12117 bname->data[cartesian_trajectory_planner_B.partialTrueCount] =
12118 body->NameInternal->
12119 data[cartesian_trajectory_planner_B.partialTrueCount];
12120 }
12121
12122 if (cartesian_trajectory_pla_strcmp(bname, endEffectorName)) {
12123 cartesian_trajectory_planner_B.bid = static_cast<real_T>
12124 (cartesian_trajectory_planner_B.i) + 1.0;
12125 exitg1 = true;
12126 } else {
12127 cartesian_trajectory_planner_B.i++;
12128 }
12129 }
12130 }
12131
12132 cartesian_traj_emxFree_char_T_a(&bname);
12133 cartesian_traj_emxFree_char_T_a(&endEffectorName);
12134 if (cartesian_trajectory_planner_B.bid == 0.0) {
12135 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices->size[0];
12136 bodyIndices->size[0] = 1;
12137 cartes_emxEnsureCapacity_real_T(bodyIndices,
12138 cartesian_trajectory_planner_B.partialTrueCount);
12139 bodyIndices->data[0] = 0.0;
12140 } else {
12141 body = obj_0->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.bid)
12142 - 1];
12143 cartesian_trajectory_planner_B.bid = 1.0;
12144 while (body->ParentIndex != 0.0) {
12145 bodyIndices->data[static_cast<int32_T>(cartesian_trajectory_planner_B.bid)
12146 - 1] = body->Index;
12147 body = obj_0->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
12148 cartesian_trajectory_planner_B.bid++;
12149 }
12150
12151 if (1.0 > cartesian_trajectory_planner_B.bid - 1.0) {
12152 cartesian_trajectory_planner_B.c_c = -1;
12153 } else {
12154 cartesian_trajectory_planner_B.c_c = static_cast<int32_T>
12155 (cartesian_trajectory_planner_B.bid - 1.0) - 1;
12156 }
12157
12158 cartesian_trajec_emxInit_real_T(&bodyIndices_0, 1);
12159 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices_0->size[0];
12160 bodyIndices_0->size[0] = cartesian_trajectory_planner_B.c_c + 3;
12161 cartes_emxEnsureCapacity_real_T(bodyIndices_0,
12162 cartesian_trajectory_planner_B.partialTrueCount);
12163 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12164 cartesian_trajectory_planner_B.partialTrueCount <=
12165 cartesian_trajectory_planner_B.c_c;
12166 cartesian_trajectory_planner_B.partialTrueCount++) {
12167 bodyIndices_0->data[cartesian_trajectory_planner_B.partialTrueCount] =
12168 bodyIndices->data[cartesian_trajectory_planner_B.partialTrueCount];
12169 }
12170
12171 bodyIndices_0->data[cartesian_trajectory_planner_B.c_c + 1] = body->Index;
12172 bodyIndices_0->data[cartesian_trajectory_planner_B.c_c + 2] = 0.0;
12173 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices->size[0];
12174 bodyIndices->size[0] = bodyIndices_0->size[0];
12175 cartes_emxEnsureCapacity_real_T(bodyIndices,
12176 cartesian_trajectory_planner_B.partialTrueCount);
12177 cartesian_trajectory_planner_B.loop_ub = bodyIndices_0->size[0];
12178 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12179 cartesian_trajectory_planner_B.partialTrueCount <
12180 cartesian_trajectory_planner_B.loop_ub;
12181 cartesian_trajectory_planner_B.partialTrueCount++) {
12182 bodyIndices->data[cartesian_trajectory_planner_B.partialTrueCount] =
12183 bodyIndices_0->data[cartesian_trajectory_planner_B.partialTrueCount];
12184 }
12185
12186 cartesian_trajec_emxFree_real_T(&bodyIndices_0);
12187 }
12188
12189 obj_0 = obj->RigidBodyTreeInternal;
12190 cartesian_trajectory_planner_B.c_c = bodyIndices->size[0] - 1;
12191 cartesian_trajectory_planner_B.loop_ub = 0;
12192 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <=
12193 cartesian_trajectory_planner_B.c_c; cartesian_trajectory_planner_B.i++) {
12194 if (bodyIndices->data[cartesian_trajectory_planner_B.i] != 0.0) {
12195 cartesian_trajectory_planner_B.loop_ub++;
12196 }
12197 }
12198
12199 cartesian_traje_emxInit_int32_T(&h, 1);
12200 cartesian_trajectory_planner_B.partialTrueCount = h->size[0];
12201 h->size[0] = cartesian_trajectory_planner_B.loop_ub;
12202 carte_emxEnsureCapacity_int32_T(h,
12203 cartesian_trajectory_planner_B.partialTrueCount);
12204 cartesian_trajectory_planner_B.partialTrueCount = 0;
12205 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <=
12206 cartesian_trajectory_planner_B.c_c; cartesian_trajectory_planner_B.i++) {
12207 if (bodyIndices->data[cartesian_trajectory_planner_B.i] != 0.0) {
12208 h->data[cartesian_trajectory_planner_B.partialTrueCount] =
12209 cartesian_trajectory_planner_B.i + 1;
12210 cartesian_trajectory_planner_B.partialTrueCount++;
12211 }
12212 }
12213
12214 cartesian_trajec_emxInit_real_T(&positionMap, 2);
12215 cartesian_trajectory_planner_B.partialTrueCount = positionMap->size[0] *
12216 positionMap->size[1];
12217 positionMap->size[0] = h->size[0];
12218 positionMap->size[1] = 2;
12219 cartes_emxEnsureCapacity_real_T(positionMap,
12220 cartesian_trajectory_planner_B.partialTrueCount);
12221 cartesian_trajectory_planner_B.loop_ub = h->size[0];
12222 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12223 cartesian_trajectory_planner_B.partialTrueCount <
12224 cartesian_trajectory_planner_B.loop_ub;
12225 cartesian_trajectory_planner_B.partialTrueCount++) {
12226 positionMap->data[cartesian_trajectory_planner_B.partialTrueCount] =
12227 obj_0->PositionDoFMap[static_cast<int32_T>(bodyIndices->data[h->
12228 data[cartesian_trajectory_planner_B.partialTrueCount] - 1]) - 1];
12229 }
12230
12231 cartesian_trajectory_planner_B.loop_ub = h->size[0];
12232 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12233 cartesian_trajectory_planner_B.partialTrueCount <
12234 cartesian_trajectory_planner_B.loop_ub;
12235 cartesian_trajectory_planner_B.partialTrueCount++) {
12236 positionMap->data[cartesian_trajectory_planner_B.partialTrueCount +
12237 positionMap->size[0]] = obj_0->PositionDoFMap[static_cast<int32_T>
12238 (bodyIndices->data[h->data[cartesian_trajectory_planner_B.partialTrueCount]
12239 - 1]) + 7];
12240 }
12241
12242 cartesian_traje_emxFree_int32_T(&h);
12243 cartesian_trajec_emxFree_real_T(&bodyIndices);
12244 cartesian_trajec_emxInit_real_T(&positionIndices, 2);
12245 cartesian_trajectory_planner_B.partialTrueCount = positionIndices->size[0] *
12246 positionIndices->size[1];
12247 positionIndices->size[0] = 1;
12248 positionIndices->size[1] = static_cast<int32_T>(obj_0->PositionNumber);
12249 cartes_emxEnsureCapacity_real_T(positionIndices,
12250 cartesian_trajectory_planner_B.partialTrueCount);
12251 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>
12252 (obj_0->PositionNumber) - 1;
12253 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12254 cartesian_trajectory_planner_B.partialTrueCount <=
12255 cartesian_trajectory_planner_B.loop_ub;
12256 cartesian_trajectory_planner_B.partialTrueCount++) {
12257 positionIndices->data[cartesian_trajectory_planner_B.partialTrueCount] = 0.0;
12258 }
12259
12260 cartesian_trajectory_planner_B.bid = 0.0;
12261 cartesian_trajectory_planner_B.c_c = positionMap->size[0] - 1;
12262 cartesian_trajec_emxInit_real_T(&e, 2);
12263 cartesian_trajec_emxInit_real_T(&y, 2);
12264 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <=
12265 cartesian_trajectory_planner_B.c_c; cartesian_trajectory_planner_B.i++) {
12266 cartesian_trajectory_planner_B.numPositions = (positionMap->
12267 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] -
12268 positionMap->data[cartesian_trajectory_planner_B.i]) + 1.0;
12269 if (cartesian_trajectory_planner_B.numPositions > 0.0) {
12270 if (cartesian_trajectory_planner_B.numPositions < 1.0) {
12271 y->size[0] = 1;
12272 y->size[1] = 0;
12273 } else if (rtIsInf(cartesian_trajectory_planner_B.numPositions) && (1.0 ==
12274 cartesian_trajectory_planner_B.numPositions)) {
12275 cartesian_trajectory_planner_B.partialTrueCount = y->size[0] * y->size[1];
12276 y->size[0] = 1;
12277 y->size[1] = 1;
12278 cartes_emxEnsureCapacity_real_T(y,
12279 cartesian_trajectory_planner_B.partialTrueCount);
12280 y->data[0] = (rtNaN);
12281 } else {
12282 cartesian_trajectory_planner_B.partialTrueCount = y->size[0] * y->size[1];
12283 y->size[0] = 1;
12284 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>(floor
12285 (cartesian_trajectory_planner_B.numPositions - 1.0));
12286 y->size[1] = cartesian_trajectory_planner_B.loop_ub + 1;
12287 cartes_emxEnsureCapacity_real_T(y,
12288 cartesian_trajectory_planner_B.partialTrueCount);
12289 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12290 cartesian_trajectory_planner_B.partialTrueCount <=
12291 cartesian_trajectory_planner_B.loop_ub;
12292 cartesian_trajectory_planner_B.partialTrueCount++) {
12293 y->data[cartesian_trajectory_planner_B.partialTrueCount] =
12294 static_cast<real_T>(cartesian_trajectory_planner_B.partialTrueCount)
12295 + 1.0;
12296 }
12297 }
12298
12299 if (rtIsNaN(positionMap->data[cartesian_trajectory_planner_B.i]) ||
12300 rtIsNaN(positionMap->data[cartesian_trajectory_planner_B.i +
12301 positionMap->size[0]])) {
12302 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
12303 e->size[0] = 1;
12304 e->size[1] = 1;
12305 cartes_emxEnsureCapacity_real_T(e,
12306 cartesian_trajectory_planner_B.partialTrueCount);
12307 e->data[0] = (rtNaN);
12308 } else if (positionMap->data[cartesian_trajectory_planner_B.i +
12309 positionMap->size[0]] < positionMap->
12310 data[cartesian_trajectory_planner_B.i]) {
12311 e->size[0] = 1;
12312 e->size[1] = 0;
12313 } else if ((rtIsInf(positionMap->data[cartesian_trajectory_planner_B.i]) ||
12314 rtIsInf(positionMap->data[cartesian_trajectory_planner_B.i +
12315 positionMap->size[0]])) && (positionMap->
12316 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] ==
12317 positionMap->data[cartesian_trajectory_planner_B.i])) {
12318 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
12319 e->size[0] = 1;
12320 e->size[1] = 1;
12321 cartes_emxEnsureCapacity_real_T(e,
12322 cartesian_trajectory_planner_B.partialTrueCount);
12323 e->data[0] = (rtNaN);
12324 } else if (floor(positionMap->data[cartesian_trajectory_planner_B.i]) ==
12325 positionMap->data[cartesian_trajectory_planner_B.i]) {
12326 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
12327 e->size[0] = 1;
12328 e->size[1] = static_cast<int32_T>(floor(positionMap->
12329 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] -
12330 positionMap->data[cartesian_trajectory_planner_B.i])) + 1;
12331 cartes_emxEnsureCapacity_real_T(e,
12332 cartesian_trajectory_planner_B.partialTrueCount);
12333 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>(floor
12334 (positionMap->data[cartesian_trajectory_planner_B.i +
12335 positionMap->size[0]] - positionMap->
12336 data[cartesian_trajectory_planner_B.i]));
12337 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12338 cartesian_trajectory_planner_B.partialTrueCount <=
12339 cartesian_trajectory_planner_B.loop_ub;
12340 cartesian_trajectory_planner_B.partialTrueCount++) {
12341 e->data[cartesian_trajectory_planner_B.partialTrueCount] =
12342 positionMap->data[cartesian_trajectory_planner_B.i] +
12343 static_cast<real_T>(cartesian_trajectory_planner_B.partialTrueCount);
12344 }
12345 } else {
12346 cartesian_trajectory_planner_B.ndbl = floor((positionMap->
12347 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] -
12348 positionMap->data[cartesian_trajectory_planner_B.i]) + 0.5);
12349 cartesian_trajectory_planner_B.apnd = positionMap->
12350 data[cartesian_trajectory_planner_B.i] +
12351 cartesian_trajectory_planner_B.ndbl;
12352 cartesian_trajectory_planner_B.cdiff =
12353 cartesian_trajectory_planner_B.apnd - positionMap->
12354 data[cartesian_trajectory_planner_B.i + positionMap->size[0]];
12355 cartesian_trajectory_planner_B.u0 = fabs(positionMap->
12356 data[cartesian_trajectory_planner_B.i]);
12357 cartesian_trajectory_planner_B.u1 = fabs(positionMap->
12358 data[cartesian_trajectory_planner_B.i + positionMap->size[0]]);
12359 if ((cartesian_trajectory_planner_B.u0 >
12360 cartesian_trajectory_planner_B.u1) || rtIsNaN
12361 (cartesian_trajectory_planner_B.u1)) {
12362 cartesian_trajectory_planner_B.u1 = cartesian_trajectory_planner_B.u0;
12363 }
12364
12365 if (fabs(cartesian_trajectory_planner_B.cdiff) < 4.4408920985006262E-16 *
12366 cartesian_trajectory_planner_B.u1) {
12367 cartesian_trajectory_planner_B.ndbl++;
12368 cartesian_trajectory_planner_B.apnd = positionMap->
12369 data[cartesian_trajectory_planner_B.i + positionMap->size[0]];
12370 } else if (cartesian_trajectory_planner_B.cdiff > 0.0) {
12371 cartesian_trajectory_planner_B.apnd =
12372 (cartesian_trajectory_planner_B.ndbl - 1.0) + positionMap->
12373 data[cartesian_trajectory_planner_B.i];
12374 } else {
12375 cartesian_trajectory_planner_B.ndbl++;
12376 }
12377
12378 if (cartesian_trajectory_planner_B.ndbl >= 0.0) {
12379 cartesian_trajectory_planner_B.partialTrueCount = static_cast<int32_T>
12380 (cartesian_trajectory_planner_B.ndbl);
12381 } else {
12382 cartesian_trajectory_planner_B.partialTrueCount = 0;
12383 }
12384
12385 cartesian_trajectory_planner_B.loop_ub =
12386 cartesian_trajectory_planner_B.partialTrueCount - 1;
12387 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
12388 e->size[0] = 1;
12389 e->size[1] = cartesian_trajectory_planner_B.loop_ub + 1;
12390 cartes_emxEnsureCapacity_real_T(e,
12391 cartesian_trajectory_planner_B.partialTrueCount);
12392 if (cartesian_trajectory_planner_B.loop_ub + 1 > 0) {
12393 e->data[0] = positionMap->data[cartesian_trajectory_planner_B.i];
12394 if (cartesian_trajectory_planner_B.loop_ub + 1 > 1) {
12395 e->data[cartesian_trajectory_planner_B.loop_ub] =
12396 cartesian_trajectory_planner_B.apnd;
12397 cartesian_trajectory_planner_B.nm1d2 =
12398 ((cartesian_trajectory_planner_B.loop_ub < 0) +
12399 cartesian_trajectory_planner_B.loop_ub) >> 1;
12400 cartesian_trajectory_planner_B.c_k =
12401 cartesian_trajectory_planner_B.nm1d2 - 2;
12402 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12403 cartesian_trajectory_planner_B.partialTrueCount <=
12404 cartesian_trajectory_planner_B.c_k;
12405 cartesian_trajectory_planner_B.partialTrueCount++) {
12406 cartesian_trajectory_planner_B.k_n =
12407 cartesian_trajectory_planner_B.partialTrueCount + 1;
12408 e->data[cartesian_trajectory_planner_B.k_n] = positionMap->
12409 data[cartesian_trajectory_planner_B.i] + static_cast<real_T>
12410 (cartesian_trajectory_planner_B.k_n);
12411 e->data[cartesian_trajectory_planner_B.loop_ub -
12412 cartesian_trajectory_planner_B.k_n] =
12413 cartesian_trajectory_planner_B.apnd - static_cast<real_T>
12414 (cartesian_trajectory_planner_B.k_n);
12415 }
12416
12417 if (cartesian_trajectory_planner_B.nm1d2 << 1 ==
12418 cartesian_trajectory_planner_B.loop_ub) {
12419 e->data[cartesian_trajectory_planner_B.nm1d2] = (positionMap->
12420 data[cartesian_trajectory_planner_B.i] +
12421 cartesian_trajectory_planner_B.apnd) / 2.0;
12422 } else {
12423 e->data[cartesian_trajectory_planner_B.nm1d2] = positionMap->
12424 data[cartesian_trajectory_planner_B.i] + static_cast<real_T>
12425 (cartesian_trajectory_planner_B.nm1d2);
12426 e->data[cartesian_trajectory_planner_B.nm1d2 + 1] =
12427 cartesian_trajectory_planner_B.apnd - static_cast<real_T>
12428 (cartesian_trajectory_planner_B.nm1d2);
12429 }
12430 }
12431 }
12432 }
12433
12434 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
12435 cartesian_trajectory_planner_B.loop_ub =
12436 cartesian_trajectory_planner_B.partialTrueCount - 1;
12437 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12438 cartesian_trajectory_planner_B.partialTrueCount <=
12439 cartesian_trajectory_planner_B.loop_ub;
12440 cartesian_trajectory_planner_B.partialTrueCount++) {
12441 positionIndices->data[static_cast<int32_T>
12442 (cartesian_trajectory_planner_B.bid + y->
12443 data[cartesian_trajectory_planner_B.partialTrueCount]) - 1] = e->
12444 data[cartesian_trajectory_planner_B.partialTrueCount];
12445 }
12446
12447 cartesian_trajectory_planner_B.bid +=
12448 cartesian_trajectory_planner_B.numPositions;
12449 }
12450 }
12451
12452 cartesian_trajec_emxFree_real_T(&y);
12453 cartesian_trajec_emxFree_real_T(&e);
12454 cartesian_trajec_emxFree_real_T(&positionMap);
12455 if (1.0 > cartesian_trajectory_planner_B.bid) {
12456 positionIndices->size[1] = 0;
12457 } else {
12458 cartesian_trajectory_planner_B.partialTrueCount = positionIndices->size[0] *
12459 positionIndices->size[1];
12460 positionIndices->size[1] = static_cast<int32_T>
12461 (cartesian_trajectory_planner_B.bid);
12462 cartes_emxEnsureCapacity_real_T(positionIndices,
12463 cartesian_trajectory_planner_B.partialTrueCount);
12464 }
12465
12466 cartesian_trajectory_planner_B.loop_ub = positionIndices->size[0] *
12467 positionIndices->size[1];
12468 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
12469 cartesian_trajectory_planner_B.partialTrueCount <
12470 cartesian_trajectory_planner_B.loop_ub;
12471 cartesian_trajectory_planner_B.partialTrueCount++) {
12472 QSol[static_cast<int32_T>(positionIndices->
12473 data[cartesian_trajectory_planner_B.partialTrueCount]) - 1] =
12474 cartesian_trajectory_planner_B.qvSolRaw[static_cast<int32_T>
12475 (positionIndices->data[cartesian_trajectory_planner_B.partialTrueCount]) -
12476 1];
12477 }
12478
12479 cartesian_trajec_emxFree_real_T(&positionIndices);
12480}
12481
12482static void cartesian_emxInit_f_cell_wrap_a(emxArray_f_cell_wrap_cartesia_T
12483 **pEmxArray, int32_T numDimensions)
12484{
12485 emxArray_f_cell_wrap_cartesia_T *emxArray;
12486 int32_T i;
12487 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)malloc(sizeof
12488 (emxArray_f_cell_wrap_cartesia_T));
12489 emxArray = *pEmxArray;
12490 emxArray->data = (f_cell_wrap_cartesian_traject_T *)NULL;
12491 emxArray->numDimensions = numDimensions;
12492 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
12493 emxArray->allocatedSize = 0;
12494 emxArray->canFreeData = true;
12495 for (i = 0; i < numDimensions; i++) {
12496 emxArray->size[i] = 0;
12497 }
12498}
12499
12500static void emxEnsureCapacity_f_cell_wrap_a(emxArray_f_cell_wrap_cartesia_T
12501 *emxArray, int32_T oldNumel)
12502{
12503 int32_T newNumel;
12504 int32_T i;
12505 void *newData;
12506 if (oldNumel < 0) {
12507 oldNumel = 0;
12508 }
12509
12510 newNumel = 1;
12511 for (i = 0; i < emxArray->numDimensions; i++) {
12512 newNumel *= emxArray->size[i];
12513 }
12514
12515 if (newNumel > emxArray->allocatedSize) {
12516 i = emxArray->allocatedSize;
12517 if (i < 16) {
12518 i = 16;
12519 }
12520
12521 while (i < newNumel) {
12522 if (i > 1073741823) {
12523 i = MAX_int32_T;
12524 } else {
12525 i <<= 1;
12526 }
12527 }
12528
12529 newData = calloc(static_cast<uint32_T>(i), sizeof
12530 (f_cell_wrap_cartesian_traject_T));
12531 if (emxArray->data != NULL) {
12532 memcpy(newData, emxArray->data, sizeof(f_cell_wrap_cartesian_traject_T)
12533 * oldNumel);
12534 if (emxArray->canFreeData) {
12535 free(emxArray->data);
12536 }
12537 }
12538
12539 emxArray->data = (f_cell_wrap_cartesian_traject_T *)newData;
12540 emxArray->allocatedSize = i;
12541 emxArray->canFreeData = true;
12542 }
12543}
12544
12545static void rigidBodyJoint_get_JointAxis_a(const c_rigidBodyJoint_cartesian__a_T
12546 *obj, real_T ax[3])
12547{
12548 emxArray_char_T_cartesian_tra_T *a;
12549 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
12550
12551 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
12552
12553 boolean_T guard1 = false;
12554 int32_T exitg1;
12555 cartesian_traj_emxInit_char_T_a(&a, 2);
12556 cartesian_trajectory_planner_B.b_kstr_c2 = a->size[0] * a->size[1];
12557 a->size[0] = 1;
12558 a->size[1] = obj->Type->size[1];
12559 cart_emxEnsureCapacity_char_T_a(a, cartesian_trajectory_planner_B.b_kstr_c2);
12560 cartesian_trajectory_planner_B.loop_ub_a = obj->Type->size[0] * obj->
12561 Type->size[1] - 1;
12562 for (cartesian_trajectory_planner_B.b_kstr_c2 = 0;
12563 cartesian_trajectory_planner_B.b_kstr_c2 <=
12564 cartesian_trajectory_planner_B.loop_ub_a;
12565 cartesian_trajectory_planner_B.b_kstr_c2++) {
12566 a->data[cartesian_trajectory_planner_B.b_kstr_c2] = obj->Type->
12567 data[cartesian_trajectory_planner_B.b_kstr_c2];
12568 }
12569
12570 for (cartesian_trajectory_planner_B.b_kstr_c2 = 0;
12571 cartesian_trajectory_planner_B.b_kstr_c2 < 8;
12572 cartesian_trajectory_planner_B.b_kstr_c2++) {
12573 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.b_kstr_c2]
12574 = tmp[cartesian_trajectory_planner_B.b_kstr_c2];
12575 }
12576
12577 cartesian_trajectory_planner_B.b_bool_g = false;
12578 if (a->size[1] == 8) {
12579 cartesian_trajectory_planner_B.b_kstr_c2 = 1;
12580 do {
12581 exitg1 = 0;
12582 if (cartesian_trajectory_planner_B.b_kstr_c2 - 1 < 8) {
12583 cartesian_trajectory_planner_B.loop_ub_a =
12584 cartesian_trajectory_planner_B.b_kstr_c2 - 1;
12585 if (a->data[cartesian_trajectory_planner_B.loop_ub_a] !=
12586 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.loop_ub_a])
12587 {
12588 exitg1 = 1;
12589 } else {
12590 cartesian_trajectory_planner_B.b_kstr_c2++;
12591 }
12592 } else {
12593 cartesian_trajectory_planner_B.b_bool_g = true;
12594 exitg1 = 1;
12595 }
12596 } while (exitg1 == 0);
12597 }
12598
12599 guard1 = false;
12600 if (cartesian_trajectory_planner_B.b_bool_g) {
12601 guard1 = true;
12602 } else {
12603 cartesian_trajectory_planner_B.b_kstr_c2 = a->size[0] * a->size[1];
12604 a->size[0] = 1;
12605 a->size[1] = obj->Type->size[1];
12606 cart_emxEnsureCapacity_char_T_a(a, cartesian_trajectory_planner_B.b_kstr_c2);
12607 cartesian_trajectory_planner_B.loop_ub_a = obj->Type->size[0] * obj->
12608 Type->size[1] - 1;
12609 for (cartesian_trajectory_planner_B.b_kstr_c2 = 0;
12610 cartesian_trajectory_planner_B.b_kstr_c2 <=
12611 cartesian_trajectory_planner_B.loop_ub_a;
12612 cartesian_trajectory_planner_B.b_kstr_c2++) {
12613 a->data[cartesian_trajectory_planner_B.b_kstr_c2] = obj->Type->
12614 data[cartesian_trajectory_planner_B.b_kstr_c2];
12615 }
12616
12617 for (cartesian_trajectory_planner_B.b_kstr_c2 = 0;
12618 cartesian_trajectory_planner_B.b_kstr_c2 < 9;
12619 cartesian_trajectory_planner_B.b_kstr_c2++) {
12620 cartesian_trajectory_planner_B.b_n[cartesian_trajectory_planner_B.b_kstr_c2]
12621 = tmp_0[cartesian_trajectory_planner_B.b_kstr_c2];
12622 }
12623
12624 cartesian_trajectory_planner_B.b_bool_g = false;
12625 if (a->size[1] == 9) {
12626 cartesian_trajectory_planner_B.b_kstr_c2 = 1;
12627 do {
12628 exitg1 = 0;
12629 if (cartesian_trajectory_planner_B.b_kstr_c2 - 1 < 9) {
12630 cartesian_trajectory_planner_B.loop_ub_a =
12631 cartesian_trajectory_planner_B.b_kstr_c2 - 1;
12632 if (a->data[cartesian_trajectory_planner_B.loop_ub_a] !=
12633 cartesian_trajectory_planner_B.b_n[cartesian_trajectory_planner_B.loop_ub_a])
12634 {
12635 exitg1 = 1;
12636 } else {
12637 cartesian_trajectory_planner_B.b_kstr_c2++;
12638 }
12639 } else {
12640 cartesian_trajectory_planner_B.b_bool_g = true;
12641 exitg1 = 1;
12642 }
12643 } while (exitg1 == 0);
12644 }
12645
12646 if (cartesian_trajectory_planner_B.b_bool_g) {
12647 guard1 = true;
12648 } else {
12649 ax[0] = (rtNaN);
12650 ax[1] = (rtNaN);
12651 ax[2] = (rtNaN);
12652 }
12653 }
12654
12655 if (guard1) {
12656 ax[0] = obj->JointAxisInternal[0];
12657 ax[1] = obj->JointAxisInternal[1];
12658 ax[2] = obj->JointAxisInternal[2];
12659 }
12660
12661 cartesian_traj_emxFree_char_T_a(&a);
12662}
12663
12664static void RigidBodyTree_forwardKinemati_a(p_robotics_manip_internal_R_a_T *obj,
12665 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree)
12666{
12667 n_robotics_manip_internal_R_a_T *body;
12668 emxArray_char_T_cartesian_tra_T *switch_expression;
12669 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
12670 };
12671
12672 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
12673
12674 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
12675
12676 int32_T exitg1;
12677 cartesian_trajectory_planner_B.n = obj->NumBodies;
12678 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
12679 cartesian_trajectory_planner_B.b_kstr_o < 16;
12680 cartesian_trajectory_planner_B.b_kstr_o++) {
12681 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.b_kstr_o]
12682 = tmp[cartesian_trajectory_planner_B.b_kstr_o];
12683 }
12684
12685 cartesian_trajectory_planner_B.b_kstr_o = Ttree->size[0] * Ttree->size[1];
12686 Ttree->size[0] = 1;
12687 cartesian_trajectory_planner_B.e_g = static_cast<int32_T>
12688 (cartesian_trajectory_planner_B.n);
12689 Ttree->size[1] = cartesian_trajectory_planner_B.e_g;
12690 emxEnsureCapacity_f_cell_wrap_a(Ttree, cartesian_trajectory_planner_B.b_kstr_o);
12691 if (cartesian_trajectory_planner_B.e_g != 0) {
12692 cartesian_trajectory_planner_B.ntilecols =
12693 cartesian_trajectory_planner_B.e_g - 1;
12694 if (0 <= cartesian_trajectory_planner_B.ntilecols) {
12695 memcpy(&cartesian_trajectory_planner_B.expl_temp.f1[0],
12696 &cartesian_trajectory_planner_B.c_f1[0], sizeof(real_T) << 4U);
12697 }
12698
12699 for (cartesian_trajectory_planner_B.b_jtilecol = 0;
12700 cartesian_trajectory_planner_B.b_jtilecol <=
12701 cartesian_trajectory_planner_B.ntilecols;
12702 cartesian_trajectory_planner_B.b_jtilecol++) {
12703 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol] =
12704 cartesian_trajectory_planner_B.expl_temp;
12705 }
12706 }
12707
12708 cartesian_trajectory_planner_B.k = 1.0;
12709 cartesian_trajectory_planner_B.ntilecols = static_cast<int32_T>
12710 (cartesian_trajectory_planner_B.n) - 1;
12711 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
12712 if (0 <= cartesian_trajectory_planner_B.ntilecols) {
12713 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
12714 cartesian_trajectory_planner_B.b_kstr_o < 5;
12715 cartesian_trajectory_planner_B.b_kstr_o++) {
12716 cartesian_trajectory_planner_B.b_ln[cartesian_trajectory_planner_B.b_kstr_o]
12717 = tmp_0[cartesian_trajectory_planner_B.b_kstr_o];
12718 }
12719 }
12720
12721 for (cartesian_trajectory_planner_B.b_jtilecol = 0;
12722 cartesian_trajectory_planner_B.b_jtilecol <=
12723 cartesian_trajectory_planner_B.ntilecols;
12724 cartesian_trajectory_planner_B.b_jtilecol++) {
12725 body = obj->Bodies[cartesian_trajectory_planner_B.b_jtilecol];
12726 cartesian_trajectory_planner_B.n = body->JointInternal.PositionNumber;
12727 cartesian_trajectory_planner_B.n += cartesian_trajectory_planner_B.k;
12728 if (cartesian_trajectory_planner_B.k > cartesian_trajectory_planner_B.n -
12729 1.0) {
12730 cartesian_trajectory_planner_B.e_g = 0;
12731 cartesian_trajectory_planner_B.d_jb = 0;
12732 } else {
12733 cartesian_trajectory_planner_B.e_g = static_cast<int32_T>
12734 (cartesian_trajectory_planner_B.k) - 1;
12735 cartesian_trajectory_planner_B.d_jb = static_cast<int32_T>
12736 (cartesian_trajectory_planner_B.n - 1.0);
12737 }
12738
12739 cartesian_trajectory_planner_B.b_kstr_o = switch_expression->size[0] *
12740 switch_expression->size[1];
12741 switch_expression->size[0] = 1;
12742 switch_expression->size[1] = body->JointInternal.Type->size[1];
12743 cart_emxEnsureCapacity_char_T_a(switch_expression,
12744 cartesian_trajectory_planner_B.b_kstr_o);
12745 cartesian_trajectory_planner_B.loop_ub_h = body->JointInternal.Type->size[0]
12746 * body->JointInternal.Type->size[1] - 1;
12747 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
12748 cartesian_trajectory_planner_B.b_kstr_o <=
12749 cartesian_trajectory_planner_B.loop_ub_h;
12750 cartesian_trajectory_planner_B.b_kstr_o++) {
12751 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_o] =
12752 body->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_o];
12753 }
12754
12755 cartesian_trajectory_planner_B.b_bool_o = false;
12756 if (switch_expression->size[1] == 5) {
12757 cartesian_trajectory_planner_B.b_kstr_o = 1;
12758 do {
12759 exitg1 = 0;
12760 if (cartesian_trajectory_planner_B.b_kstr_o - 1 < 5) {
12761 cartesian_trajectory_planner_B.loop_ub_h =
12762 cartesian_trajectory_planner_B.b_kstr_o - 1;
12763 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_h]
12764 !=
12765 cartesian_trajectory_planner_B.b_ln[cartesian_trajectory_planner_B.loop_ub_h])
12766 {
12767 exitg1 = 1;
12768 } else {
12769 cartesian_trajectory_planner_B.b_kstr_o++;
12770 }
12771 } else {
12772 cartesian_trajectory_planner_B.b_bool_o = true;
12773 exitg1 = 1;
12774 }
12775 } while (exitg1 == 0);
12776 }
12777
12778 if (cartesian_trajectory_planner_B.b_bool_o) {
12779 cartesian_trajectory_planner_B.b_kstr_o = 0;
12780 } else {
12781 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
12782 cartesian_trajectory_planner_B.b_kstr_o < 8;
12783 cartesian_trajectory_planner_B.b_kstr_o++) {
12784 cartesian_trajectory_planner_B.b_k[cartesian_trajectory_planner_B.b_kstr_o]
12785 = tmp_1[cartesian_trajectory_planner_B.b_kstr_o];
12786 }
12787
12788 cartesian_trajectory_planner_B.b_bool_o = false;
12789 if (switch_expression->size[1] == 8) {
12790 cartesian_trajectory_planner_B.b_kstr_o = 1;
12791 do {
12792 exitg1 = 0;
12793 if (cartesian_trajectory_planner_B.b_kstr_o - 1 < 8) {
12794 cartesian_trajectory_planner_B.loop_ub_h =
12795 cartesian_trajectory_planner_B.b_kstr_o - 1;
12796 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_h]
12797 !=
12798 cartesian_trajectory_planner_B.b_k[cartesian_trajectory_planner_B.loop_ub_h])
12799 {
12800 exitg1 = 1;
12801 } else {
12802 cartesian_trajectory_planner_B.b_kstr_o++;
12803 }
12804 } else {
12805 cartesian_trajectory_planner_B.b_bool_o = true;
12806 exitg1 = 1;
12807 }
12808 } while (exitg1 == 0);
12809 }
12810
12811 if (cartesian_trajectory_planner_B.b_bool_o) {
12812 cartesian_trajectory_planner_B.b_kstr_o = 1;
12813 } else {
12814 cartesian_trajectory_planner_B.b_kstr_o = -1;
12815 }
12816 }
12817
12818 switch (cartesian_trajectory_planner_B.b_kstr_o) {
12819 case 0:
12820 memset(&cartesian_trajectory_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
12821 cartesian_trajectory_planner_B.c_f1[0] = 1.0;
12822 cartesian_trajectory_planner_B.c_f1[5] = 1.0;
12823 cartesian_trajectory_planner_B.c_f1[10] = 1.0;
12824 cartesian_trajectory_planner_B.c_f1[15] = 1.0;
12825 break;
12826
12827 case 1:
12828 rigidBodyJoint_get_JointAxis_a(&body->JointInternal,
12829 cartesian_trajectory_planner_B.v_my);
12830 cartesian_trajectory_planner_B.d_jb -= cartesian_trajectory_planner_B.e_g;
12831 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
12832 cartesian_trajectory_planner_B.b_kstr_o <
12833 cartesian_trajectory_planner_B.d_jb;
12834 cartesian_trajectory_planner_B.b_kstr_o++) {
12835 cartesian_trajectory_planner_B.e_data[cartesian_trajectory_planner_B.b_kstr_o]
12836 = cartesian_trajectory_planner_B.e_g +
12837 cartesian_trajectory_planner_B.b_kstr_o;
12838 }
12839
12840 cartesian_trajectory_planner_B.result_data_m[0] =
12841 cartesian_trajectory_planner_B.v_my[0];
12842 cartesian_trajectory_planner_B.result_data_m[1] =
12843 cartesian_trajectory_planner_B.v_my[1];
12844 cartesian_trajectory_planner_B.result_data_m[2] =
12845 cartesian_trajectory_planner_B.v_my[2];
12846 if (0 <= (cartesian_trajectory_planner_B.d_jb != 0) - 1) {
12847 cartesian_trajectory_planner_B.result_data_m[3] =
12848 qvec[cartesian_trajectory_planner_B.e_data[0]];
12849 }
12850
12851 cartesian_trajectory_planner_B.k = 1.0 / sqrt
12852 ((cartesian_trajectory_planner_B.result_data_m[0] *
12853 cartesian_trajectory_planner_B.result_data_m[0] +
12854 cartesian_trajectory_planner_B.result_data_m[1] *
12855 cartesian_trajectory_planner_B.result_data_m[1]) +
12856 cartesian_trajectory_planner_B.result_data_m[2] *
12857 cartesian_trajectory_planner_B.result_data_m[2]);
12858 cartesian_trajectory_planner_B.v_my[0] =
12859 cartesian_trajectory_planner_B.result_data_m[0] *
12860 cartesian_trajectory_planner_B.k;
12861 cartesian_trajectory_planner_B.v_my[1] =
12862 cartesian_trajectory_planner_B.result_data_m[1] *
12863 cartesian_trajectory_planner_B.k;
12864 cartesian_trajectory_planner_B.v_my[2] =
12865 cartesian_trajectory_planner_B.result_data_m[2] *
12866 cartesian_trajectory_planner_B.k;
12867 cartesian_trajectory_planner_B.k = cos
12868 (cartesian_trajectory_planner_B.result_data_m[3]);
12869 cartesian_trajectory_planner_B.sth_p = sin
12870 (cartesian_trajectory_planner_B.result_data_m[3]);
12871 cartesian_trajectory_planner_B.tempR_m[0] =
12872 cartesian_trajectory_planner_B.v_my[0] *
12873 cartesian_trajectory_planner_B.v_my[0] * (1.0 -
12874 cartesian_trajectory_planner_B.k) + cartesian_trajectory_planner_B.k;
12875 cartesian_trajectory_planner_B.tempR_tmp_dk =
12876 cartesian_trajectory_planner_B.v_my[1] *
12877 cartesian_trajectory_planner_B.v_my[0] * (1.0 -
12878 cartesian_trajectory_planner_B.k);
12879 cartesian_trajectory_planner_B.tempR_tmp_o =
12880 cartesian_trajectory_planner_B.v_my[2] *
12881 cartesian_trajectory_planner_B.sth_p;
12882 cartesian_trajectory_planner_B.tempR_m[1] =
12883 cartesian_trajectory_planner_B.tempR_tmp_dk -
12884 cartesian_trajectory_planner_B.tempR_tmp_o;
12885 cartesian_trajectory_planner_B.tempR_tmp_jr =
12886 cartesian_trajectory_planner_B.v_my[2] *
12887 cartesian_trajectory_planner_B.v_my[0] * (1.0 -
12888 cartesian_trajectory_planner_B.k);
12889 cartesian_trajectory_planner_B.tempR_tmp_c =
12890 cartesian_trajectory_planner_B.v_my[1] *
12891 cartesian_trajectory_planner_B.sth_p;
12892 cartesian_trajectory_planner_B.tempR_m[2] =
12893 cartesian_trajectory_planner_B.tempR_tmp_jr +
12894 cartesian_trajectory_planner_B.tempR_tmp_c;
12895 cartesian_trajectory_planner_B.tempR_m[3] =
12896 cartesian_trajectory_planner_B.tempR_tmp_dk +
12897 cartesian_trajectory_planner_B.tempR_tmp_o;
12898 cartesian_trajectory_planner_B.tempR_m[4] =
12899 cartesian_trajectory_planner_B.v_my[1] *
12900 cartesian_trajectory_planner_B.v_my[1] * (1.0 -
12901 cartesian_trajectory_planner_B.k) + cartesian_trajectory_planner_B.k;
12902 cartesian_trajectory_planner_B.tempR_tmp_dk =
12903 cartesian_trajectory_planner_B.v_my[2] *
12904 cartesian_trajectory_planner_B.v_my[1] * (1.0 -
12905 cartesian_trajectory_planner_B.k);
12906 cartesian_trajectory_planner_B.tempR_tmp_o =
12907 cartesian_trajectory_planner_B.v_my[0] *
12908 cartesian_trajectory_planner_B.sth_p;
12909 cartesian_trajectory_planner_B.tempR_m[5] =
12910 cartesian_trajectory_planner_B.tempR_tmp_dk -
12911 cartesian_trajectory_planner_B.tempR_tmp_o;
12912 cartesian_trajectory_planner_B.tempR_m[6] =
12913 cartesian_trajectory_planner_B.tempR_tmp_jr -
12914 cartesian_trajectory_planner_B.tempR_tmp_c;
12915 cartesian_trajectory_planner_B.tempR_m[7] =
12916 cartesian_trajectory_planner_B.tempR_tmp_dk +
12917 cartesian_trajectory_planner_B.tempR_tmp_o;
12918 cartesian_trajectory_planner_B.tempR_m[8] =
12919 cartesian_trajectory_planner_B.v_my[2] *
12920 cartesian_trajectory_planner_B.v_my[2] * (1.0 -
12921 cartesian_trajectory_planner_B.k) + cartesian_trajectory_planner_B.k;
12922 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
12923 cartesian_trajectory_planner_B.b_kstr_o < 3;
12924 cartesian_trajectory_planner_B.b_kstr_o++) {
12925 cartesian_trajectory_planner_B.e_g =
12926 cartesian_trajectory_planner_B.b_kstr_o + 1;
12927 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.e_g -
12928 1] = cartesian_trajectory_planner_B.tempR_m
12929 [(cartesian_trajectory_planner_B.e_g - 1) * 3];
12930 cartesian_trajectory_planner_B.e_g =
12931 cartesian_trajectory_planner_B.b_kstr_o + 1;
12932 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.e_g +
12933 2] = cartesian_trajectory_planner_B.tempR_m
12934 [(cartesian_trajectory_planner_B.e_g - 1) * 3 + 1];
12935 cartesian_trajectory_planner_B.e_g =
12936 cartesian_trajectory_planner_B.b_kstr_o + 1;
12937 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.e_g +
12938 5] = cartesian_trajectory_planner_B.tempR_m
12939 [(cartesian_trajectory_planner_B.e_g - 1) * 3 + 2];
12940 }
12941
12942 memset(&cartesian_trajectory_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
12943 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
12944 cartesian_trajectory_planner_B.b_kstr_o < 3;
12945 cartesian_trajectory_planner_B.b_kstr_o++) {
12946 cartesian_trajectory_planner_B.d_jb =
12947 cartesian_trajectory_planner_B.b_kstr_o << 2;
12948 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb]
12949 = cartesian_trajectory_planner_B.R_l[3 *
12950 cartesian_trajectory_planner_B.b_kstr_o];
12951 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb
12952 + 1] = cartesian_trajectory_planner_B.R_l[3 *
12953 cartesian_trajectory_planner_B.b_kstr_o + 1];
12954 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb
12955 + 2] = cartesian_trajectory_planner_B.R_l[3 *
12956 cartesian_trajectory_planner_B.b_kstr_o + 2];
12957 }
12958
12959 cartesian_trajectory_planner_B.c_f1[15] = 1.0;
12960 break;
12961
12962 default:
12963 rigidBodyJoint_get_JointAxis_a(&body->JointInternal,
12964 cartesian_trajectory_planner_B.v_my);
12965 memset(&cartesian_trajectory_planner_B.tempR_m[0], 0, 9U * sizeof(real_T));
12966 cartesian_trajectory_planner_B.tempR_m[0] = 1.0;
12967 cartesian_trajectory_planner_B.tempR_m[4] = 1.0;
12968 cartesian_trajectory_planner_B.tempR_m[8] = 1.0;
12969 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
12970 cartesian_trajectory_planner_B.b_kstr_o < 3;
12971 cartesian_trajectory_planner_B.b_kstr_o++) {
12972 cartesian_trajectory_planner_B.d_jb =
12973 cartesian_trajectory_planner_B.b_kstr_o << 2;
12974 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb]
12975 = cartesian_trajectory_planner_B.tempR_m[3 *
12976 cartesian_trajectory_planner_B.b_kstr_o];
12977 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb
12978 + 1] = cartesian_trajectory_planner_B.tempR_m[3 *
12979 cartesian_trajectory_planner_B.b_kstr_o + 1];
12980 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb
12981 + 2] = cartesian_trajectory_planner_B.tempR_m[3 *
12982 cartesian_trajectory_planner_B.b_kstr_o + 2];
12983 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.b_kstr_o
12984 + 12] =
12985 cartesian_trajectory_planner_B.v_my[cartesian_trajectory_planner_B.b_kstr_o]
12986 * qvec[cartesian_trajectory_planner_B.e_g];
12987 }
12988
12989 cartesian_trajectory_planner_B.c_f1[3] = 0.0;
12990 cartesian_trajectory_planner_B.c_f1[7] = 0.0;
12991 cartesian_trajectory_planner_B.c_f1[11] = 0.0;
12992 cartesian_trajectory_planner_B.c_f1[15] = 1.0;
12993 break;
12994 }
12995
12996 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
12997 cartesian_trajectory_planner_B.b_kstr_o < 16;
12998 cartesian_trajectory_planner_B.b_kstr_o++) {
12999 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o]
13000 = body->
13001 JointInternal.JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_o];
13002 }
13003
13004 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
13005 cartesian_trajectory_planner_B.b_kstr_o < 16;
13006 cartesian_trajectory_planner_B.b_kstr_o++) {
13007 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.b_kstr_o]
13008 = body->
13009 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_o];
13010 }
13011
13012 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
13013 cartesian_trajectory_planner_B.b_kstr_o < 4;
13014 cartesian_trajectory_planner_B.b_kstr_o++) {
13015 for (cartesian_trajectory_planner_B.e_g = 0;
13016 cartesian_trajectory_planner_B.e_g < 4;
13017 cartesian_trajectory_planner_B.e_g++) {
13018 cartesian_trajectory_planner_B.d_jb = cartesian_trajectory_planner_B.e_g
13019 << 2;
13020 cartesian_trajectory_planner_B.loop_ub_h =
13021 cartesian_trajectory_planner_B.b_kstr_o +
13022 cartesian_trajectory_planner_B.d_jb;
13023 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13024 = 0.0;
13025 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13026 +=
13027 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb]
13028 * cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o];
13029 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13030 +=
13031 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb
13032 + 1] *
13033 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o
13034 + 4];
13035 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13036 +=
13037 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb
13038 + 2] *
13039 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o
13040 + 8];
13041 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13042 +=
13043 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_jb
13044 + 3] *
13045 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o
13046 + 12];
13047 }
13048
13049 for (cartesian_trajectory_planner_B.e_g = 0;
13050 cartesian_trajectory_planner_B.e_g < 4;
13051 cartesian_trajectory_planner_B.e_g++) {
13052 cartesian_trajectory_planner_B.d_jb = cartesian_trajectory_planner_B.e_g
13053 << 2;
13054 cartesian_trajectory_planner_B.loop_ub_h =
13055 cartesian_trajectory_planner_B.b_kstr_o +
13056 cartesian_trajectory_planner_B.d_jb;
13057 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13058 .f1[cartesian_trajectory_planner_B.loop_ub_h] = 0.0;
13059 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13060 .f1[cartesian_trajectory_planner_B.loop_ub_h] +=
13061 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.d_jb]
13062 * cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_o];
13063 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13064 .f1[cartesian_trajectory_planner_B.loop_ub_h] +=
13065 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.d_jb
13066 + 1] *
13067 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_o
13068 + 4];
13069 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13070 .f1[cartesian_trajectory_planner_B.loop_ub_h] +=
13071 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.d_jb
13072 + 2] *
13073 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_o
13074 + 8];
13075 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13076 .f1[cartesian_trajectory_planner_B.loop_ub_h] +=
13077 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.d_jb
13078 + 3] *
13079 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_o
13080 + 12];
13081 }
13082 }
13083
13084 cartesian_trajectory_planner_B.k = cartesian_trajectory_planner_B.n;
13085 if (body->ParentIndex > 0.0) {
13086 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
13087 cartesian_trajectory_planner_B.b_kstr_o < 16;
13088 cartesian_trajectory_planner_B.b_kstr_o++) {
13089 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o]
13090 = Ttree->data[static_cast<int32_T>(body->ParentIndex) - 1]
13091 .f1[cartesian_trajectory_planner_B.b_kstr_o];
13092 }
13093
13094 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
13095 cartesian_trajectory_planner_B.b_kstr_o < 4;
13096 cartesian_trajectory_planner_B.b_kstr_o++) {
13097 for (cartesian_trajectory_planner_B.e_g = 0;
13098 cartesian_trajectory_planner_B.e_g < 4;
13099 cartesian_trajectory_planner_B.e_g++) {
13100 cartesian_trajectory_planner_B.d_jb =
13101 cartesian_trajectory_planner_B.e_g << 2;
13102 cartesian_trajectory_planner_B.loop_ub_h =
13103 cartesian_trajectory_planner_B.b_kstr_o +
13104 cartesian_trajectory_planner_B.d_jb;
13105 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13106 = 0.0;
13107 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13108 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13109 .f1[cartesian_trajectory_planner_B.d_jb] *
13110 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o];
13111 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13112 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13113 .f1[cartesian_trajectory_planner_B.d_jb + 1] *
13114 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o
13115 + 4];
13116 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13117 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13118 .f1[cartesian_trajectory_planner_B.d_jb + 2] *
13119 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o
13120 + 8];
13121 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_h]
13122 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13123 .f1[cartesian_trajectory_planner_B.d_jb + 3] *
13124 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_o
13125 + 12];
13126 }
13127 }
13128
13129 for (cartesian_trajectory_planner_B.b_kstr_o = 0;
13130 cartesian_trajectory_planner_B.b_kstr_o < 16;
13131 cartesian_trajectory_planner_B.b_kstr_o++) {
13132 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
13133 .f1[cartesian_trajectory_planner_B.b_kstr_o] =
13134 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_o];
13135 }
13136 }
13137 }
13138
13139 cartesian_traj_emxFree_char_T_a(&switch_expression);
13140}
13141
13142static void cartesian_emxFree_f_cell_wrap_a(emxArray_f_cell_wrap_cartesia_T
13143 **pEmxArray)
13144{
13145 if (*pEmxArray != (emxArray_f_cell_wrap_cartesia_T *)NULL) {
13146 if (((*pEmxArray)->data != (f_cell_wrap_cartesian_traject_T *)NULL) &&
13147 (*pEmxArray)->canFreeData) {
13148 free((*pEmxArray)->data);
13149 }
13150
13151 free((*pEmxArray)->size);
13152 free(*pEmxArray);
13153 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)NULL;
13154 }
13155}
13156
13157static void RigidBodyTree_geometricJacobian(p_robotics_manip_internal_R_a_T *obj,
13158 const real_T Q[6], emxArray_real_T_cartesian_tra_T *Jac)
13159{
13160 emxArray_f_cell_wrap_cartesia_T *Ttree;
13161 n_robotics_manip_internal_R_a_T *body;
13162 emxArray_real_T_cartesian_tra_T *JacSlice;
13163 emxArray_char_T_cartesian_tra_T *bname;
13164 emxArray_real_T_cartesian_tra_T *b;
13165 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
13166 'e', 'e' };
13167
13168 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
13169
13170 int32_T exitg1;
13171 boolean_T exitg2;
13172 cartesian_emxInit_f_cell_wrap_a(&Ttree, 2);
13173 RigidBodyTree_forwardKinemati_a(obj, Q, Ttree);
13174 cartesian_trajectory_planner_B.b_kstr_ga = Jac->size[0] * Jac->size[1];
13175 Jac->size[0] = 6;
13176 Jac->size[1] = static_cast<int32_T>(obj->VelocityNumber);
13177 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr_ga);
13178 cartesian_trajectory_planner_B.loop_ub_e = 6 * static_cast<int32_T>
13179 (obj->VelocityNumber) - 1;
13180 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13181 cartesian_trajectory_planner_B.b_kstr_ga <=
13182 cartesian_trajectory_planner_B.loop_ub_e;
13183 cartesian_trajectory_planner_B.b_kstr_ga++) {
13184 Jac->data[cartesian_trajectory_planner_B.b_kstr_ga] = 0.0;
13185 }
13186
13187 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13188 cartesian_trajectory_planner_B.b_kstr_ga < 8;
13189 cartesian_trajectory_planner_B.b_kstr_ga++) {
13190 cartesian_trajectory_planner_B.chainmask[cartesian_trajectory_planner_B.b_kstr_ga]
13191 = 0;
13192 }
13193
13194 cartesian_traj_emxInit_char_T_a(&bname, 2);
13195 cartesian_trajectory_planner_B.b_kstr_ga = bname->size[0] * bname->size[1];
13196 bname->size[0] = 1;
13197 bname->size[1] = obj->Base.NameInternal->size[1];
13198 cart_emxEnsureCapacity_char_T_a(bname,
13199 cartesian_trajectory_planner_B.b_kstr_ga);
13200 cartesian_trajectory_planner_B.loop_ub_e = obj->Base.NameInternal->size[0] *
13201 obj->Base.NameInternal->size[1] - 1;
13202 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13203 cartesian_trajectory_planner_B.b_kstr_ga <=
13204 cartesian_trajectory_planner_B.loop_ub_e;
13205 cartesian_trajectory_planner_B.b_kstr_ga++) {
13206 bname->data[cartesian_trajectory_planner_B.b_kstr_ga] =
13207 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_ga];
13208 }
13209
13210 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13211 cartesian_trajectory_planner_B.b_kstr_ga < 11;
13212 cartesian_trajectory_planner_B.b_kstr_ga++) {
13213 cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.b_kstr_ga]
13214 = tmp[cartesian_trajectory_planner_B.b_kstr_ga];
13215 }
13216
13217 cartesian_trajectory_planner_B.b_bool_b = false;
13218 if (11 == bname->size[1]) {
13219 cartesian_trajectory_planner_B.b_kstr_ga = 1;
13220 do {
13221 exitg1 = 0;
13222 if (cartesian_trajectory_planner_B.b_kstr_ga - 1 < 11) {
13223 cartesian_trajectory_planner_B.kstr =
13224 cartesian_trajectory_planner_B.b_kstr_ga - 1;
13225 if (cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.kstr]
13226 != bname->data[cartesian_trajectory_planner_B.kstr]) {
13227 exitg1 = 1;
13228 } else {
13229 cartesian_trajectory_planner_B.b_kstr_ga++;
13230 }
13231 } else {
13232 cartesian_trajectory_planner_B.b_bool_b = true;
13233 exitg1 = 1;
13234 }
13235 } while (exitg1 == 0);
13236 }
13237
13238 if (cartesian_trajectory_planner_B.b_bool_b) {
13239 memset(&cartesian_trajectory_planner_B.T2inv[0], 0, sizeof(real_T) << 4U);
13240 cartesian_trajectory_planner_B.T2inv[0] = 1.0;
13241 cartesian_trajectory_planner_B.T2inv[5] = 1.0;
13242 cartesian_trajectory_planner_B.T2inv[10] = 1.0;
13243 cartesian_trajectory_planner_B.T2inv[15] = 1.0;
13244 memset(&cartesian_trajectory_planner_B.T2[0], 0, sizeof(real_T) << 4U);
13245 cartesian_trajectory_planner_B.T2[0] = 1.0;
13246 cartesian_trajectory_planner_B.T2[5] = 1.0;
13247 cartesian_trajectory_planner_B.T2[10] = 1.0;
13248 cartesian_trajectory_planner_B.T2[15] = 1.0;
13249 } else {
13250 cartesian_trajectory_planner_B.endeffectorIndex = -1.0;
13251 cartesian_trajectory_planner_B.b_kstr_ga = bname->size[0] * bname->size[1];
13252 bname->size[0] = 1;
13253 bname->size[1] = obj->Base.NameInternal->size[1];
13254 cart_emxEnsureCapacity_char_T_a(bname,
13255 cartesian_trajectory_planner_B.b_kstr_ga);
13256 cartesian_trajectory_planner_B.loop_ub_e = obj->Base.NameInternal->size[0] *
13257 obj->Base.NameInternal->size[1] - 1;
13258 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13259 cartesian_trajectory_planner_B.b_kstr_ga <=
13260 cartesian_trajectory_planner_B.loop_ub_e;
13261 cartesian_trajectory_planner_B.b_kstr_ga++) {
13262 bname->data[cartesian_trajectory_planner_B.b_kstr_ga] =
13263 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_ga];
13264 }
13265
13266 cartesian_trajectory_planner_B.b_bool_b = false;
13267 if (bname->size[1] == 11) {
13268 cartesian_trajectory_planner_B.b_kstr_ga = 1;
13269 do {
13270 exitg1 = 0;
13271 if (cartesian_trajectory_planner_B.b_kstr_ga - 1 < 11) {
13272 cartesian_trajectory_planner_B.kstr =
13273 cartesian_trajectory_planner_B.b_kstr_ga - 1;
13274 if (bname->data[cartesian_trajectory_planner_B.kstr] !=
13275 cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.kstr])
13276 {
13277 exitg1 = 1;
13278 } else {
13279 cartesian_trajectory_planner_B.b_kstr_ga++;
13280 }
13281 } else {
13282 cartesian_trajectory_planner_B.b_bool_b = true;
13283 exitg1 = 1;
13284 }
13285 } while (exitg1 == 0);
13286 }
13287
13288 if (cartesian_trajectory_planner_B.b_bool_b) {
13289 cartesian_trajectory_planner_B.endeffectorIndex = 0.0;
13290 } else {
13291 cartesian_trajectory_planner_B.idx_idx_1 = obj->NumBodies;
13292 cartesian_trajectory_planner_B.b_i_j = 0;
13293 exitg2 = false;
13294 while ((!exitg2) && (cartesian_trajectory_planner_B.b_i_j <=
13295 static_cast<int32_T>
13296 (cartesian_trajectory_planner_B.idx_idx_1) - 1)) {
13297 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_j];
13298 cartesian_trajectory_planner_B.b_kstr_ga = bname->size[0] * bname->size
13299 [1];
13300 bname->size[0] = 1;
13301 bname->size[1] = body->NameInternal->size[1];
13302 cart_emxEnsureCapacity_char_T_a(bname,
13303 cartesian_trajectory_planner_B.b_kstr_ga);
13304 cartesian_trajectory_planner_B.loop_ub_e = body->NameInternal->size[0] *
13305 body->NameInternal->size[1] - 1;
13306 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13307 cartesian_trajectory_planner_B.b_kstr_ga <=
13308 cartesian_trajectory_planner_B.loop_ub_e;
13309 cartesian_trajectory_planner_B.b_kstr_ga++) {
13310 bname->data[cartesian_trajectory_planner_B.b_kstr_ga] =
13311 body->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_ga];
13312 }
13313
13314 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13315 cartesian_trajectory_planner_B.b_kstr_ga < 11;
13316 cartesian_trajectory_planner_B.b_kstr_ga++) {
13317 cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.b_kstr_ga]
13318 = tmp[cartesian_trajectory_planner_B.b_kstr_ga];
13319 }
13320
13321 cartesian_trajectory_planner_B.b_bool_b = false;
13322 if (bname->size[1] == 11) {
13323 cartesian_trajectory_planner_B.b_kstr_ga = 1;
13324 do {
13325 exitg1 = 0;
13326 if (cartesian_trajectory_planner_B.b_kstr_ga - 1 < 11) {
13327 cartesian_trajectory_planner_B.kstr =
13328 cartesian_trajectory_planner_B.b_kstr_ga - 1;
13329 if (bname->data[cartesian_trajectory_planner_B.kstr] !=
13330 cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.kstr])
13331 {
13332 exitg1 = 1;
13333 } else {
13334 cartesian_trajectory_planner_B.b_kstr_ga++;
13335 }
13336 } else {
13337 cartesian_trajectory_planner_B.b_bool_b = true;
13338 exitg1 = 1;
13339 }
13340 } while (exitg1 == 0);
13341 }
13342
13343 if (cartesian_trajectory_planner_B.b_bool_b) {
13344 cartesian_trajectory_planner_B.endeffectorIndex = static_cast<real_T>
13345 (cartesian_trajectory_planner_B.b_i_j) + 1.0;
13346 exitg2 = true;
13347 } else {
13348 cartesian_trajectory_planner_B.b_i_j++;
13349 }
13350 }
13351 }
13352
13353 cartesian_trajectory_planner_B.b_i_j = static_cast<int32_T>
13354 (cartesian_trajectory_planner_B.endeffectorIndex) - 1;
13355 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_j];
13356 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13357 cartesian_trajectory_planner_B.b_kstr_ga < 16;
13358 cartesian_trajectory_planner_B.b_kstr_ga++) {
13359 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_kstr_ga]
13360 = Ttree->data[cartesian_trajectory_planner_B.b_i_j]
13361 .f1[cartesian_trajectory_planner_B.b_kstr_ga];
13362 }
13363
13364 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13365 cartesian_trajectory_planner_B.b_kstr_ga < 3;
13366 cartesian_trajectory_planner_B.b_kstr_ga++) {
13367 cartesian_trajectory_planner_B.R_c[3 *
13368 cartesian_trajectory_planner_B.b_kstr_ga] = Ttree->
13369 data[cartesian_trajectory_planner_B.b_i_j]
13370 .f1[cartesian_trajectory_planner_B.b_kstr_ga];
13371 cartesian_trajectory_planner_B.R_c[3 *
13372 cartesian_trajectory_planner_B.b_kstr_ga + 1] = Ttree->
13373 data[cartesian_trajectory_planner_B.b_i_j]
13374 .f1[cartesian_trajectory_planner_B.b_kstr_ga + 4];
13375 cartesian_trajectory_planner_B.R_c[3 *
13376 cartesian_trajectory_planner_B.b_kstr_ga + 2] = Ttree->
13377 data[cartesian_trajectory_planner_B.b_i_j]
13378 .f1[cartesian_trajectory_planner_B.b_kstr_ga + 8];
13379 }
13380
13381 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13382 cartesian_trajectory_planner_B.b_kstr_ga < 9;
13383 cartesian_trajectory_planner_B.b_kstr_ga++) {
13384 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.b_kstr_ga]
13385 =
13386 -cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr_ga];
13387 }
13388
13389 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13390 cartesian_trajectory_planner_B.b_kstr_ga < 3;
13391 cartesian_trajectory_planner_B.b_kstr_ga++) {
13392 cartesian_trajectory_planner_B.endeffectorIndex = Ttree->
13393 data[cartesian_trajectory_planner_B.b_i_j].f1[12] *
13394 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.b_kstr_ga];
13395 cartesian_trajectory_planner_B.loop_ub_e =
13396 cartesian_trajectory_planner_B.b_kstr_ga << 2;
13397 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.loop_ub_e]
13398 = cartesian_trajectory_planner_B.R_c[3 *
13399 cartesian_trajectory_planner_B.b_kstr_ga];
13400 cartesian_trajectory_planner_B.endeffectorIndex +=
13401 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.b_kstr_ga
13402 + 3] * Ttree->data[cartesian_trajectory_planner_B.b_i_j].f1[13];
13403 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.loop_ub_e
13404 + 1] = cartesian_trajectory_planner_B.R_c[3 *
13405 cartesian_trajectory_planner_B.b_kstr_ga + 1];
13406 cartesian_trajectory_planner_B.endeffectorIndex +=
13407 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.b_kstr_ga
13408 + 6] * Ttree->data[cartesian_trajectory_planner_B.b_i_j].f1[14];
13409 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.loop_ub_e
13410 + 2] = cartesian_trajectory_planner_B.R_c[3 *
13411 cartesian_trajectory_planner_B.b_kstr_ga + 2];
13412 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_ga
13413 + 12] = cartesian_trajectory_planner_B.endeffectorIndex;
13414 }
13415
13416 cartesian_trajectory_planner_B.T2inv[3] = 0.0;
13417 cartesian_trajectory_planner_B.T2inv[7] = 0.0;
13418 cartesian_trajectory_planner_B.T2inv[11] = 0.0;
13419 cartesian_trajectory_planner_B.T2inv[15] = 1.0;
13420 cartesian_trajectory_planner_B.chainmask[cartesian_trajectory_planner_B.b_i_j]
13421 = 1;
13422 while (body->ParentIndex > 0.0) {
13423 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
13424 cartesian_trajectory_planner_B.chainmask[static_cast<int32_T>(body->Index)
13425 - 1] = 1;
13426 }
13427 }
13428
13429 cartesian_trajectory_planner_B.idx_idx_1 = obj->NumBodies;
13430 cartesian_trajectory_planner_B.c_ai = static_cast<int32_T>
13431 (cartesian_trajectory_planner_B.idx_idx_1) - 1;
13432 cartesian_trajec_emxInit_real_T(&JacSlice, 2);
13433 cartesian_trajec_emxInit_real_T(&b, 2);
13434 if (0 <= cartesian_trajectory_planner_B.c_ai) {
13435 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13436 cartesian_trajectory_planner_B.b_kstr_ga < 5;
13437 cartesian_trajectory_planner_B.b_kstr_ga++) {
13438 cartesian_trajectory_planner_B.b_oy[cartesian_trajectory_planner_B.b_kstr_ga]
13439 = tmp_0[cartesian_trajectory_planner_B.b_kstr_ga];
13440 }
13441 }
13442
13443 for (cartesian_trajectory_planner_B.b_i_j = 0;
13444 cartesian_trajectory_planner_B.b_i_j <=
13445 cartesian_trajectory_planner_B.c_ai; cartesian_trajectory_planner_B.b_i_j
13446 ++) {
13447 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_j];
13448 cartesian_trajectory_planner_B.b_kstr_ga = bname->size[0] * bname->size[1];
13449 bname->size[0] = 1;
13450 bname->size[1] = body->JointInternal.Type->size[1];
13451 cart_emxEnsureCapacity_char_T_a(bname,
13452 cartesian_trajectory_planner_B.b_kstr_ga);
13453 cartesian_trajectory_planner_B.loop_ub_e = body->JointInternal.Type->size[0]
13454 * body->JointInternal.Type->size[1] - 1;
13455 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13456 cartesian_trajectory_planner_B.b_kstr_ga <=
13457 cartesian_trajectory_planner_B.loop_ub_e;
13458 cartesian_trajectory_planner_B.b_kstr_ga++) {
13459 bname->data[cartesian_trajectory_planner_B.b_kstr_ga] =
13460 body->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_ga];
13461 }
13462
13463 cartesian_trajectory_planner_B.b_bool_b = false;
13464 if (bname->size[1] == 5) {
13465 cartesian_trajectory_planner_B.b_kstr_ga = 1;
13466 do {
13467 exitg1 = 0;
13468 if (cartesian_trajectory_planner_B.b_kstr_ga - 1 < 5) {
13469 cartesian_trajectory_planner_B.kstr =
13470 cartesian_trajectory_planner_B.b_kstr_ga - 1;
13471 if (bname->data[cartesian_trajectory_planner_B.kstr] !=
13472 cartesian_trajectory_planner_B.b_oy[cartesian_trajectory_planner_B.kstr])
13473 {
13474 exitg1 = 1;
13475 } else {
13476 cartesian_trajectory_planner_B.b_kstr_ga++;
13477 }
13478 } else {
13479 cartesian_trajectory_planner_B.b_bool_b = true;
13480 exitg1 = 1;
13481 }
13482 } while (exitg1 == 0);
13483 }
13484
13485 if ((!cartesian_trajectory_planner_B.b_bool_b) &&
13486 (cartesian_trajectory_planner_B.chainmask[cartesian_trajectory_planner_B.b_i_j]
13487 != 0)) {
13488 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13489 cartesian_trajectory_planner_B.b_kstr_ga < 16;
13490 cartesian_trajectory_planner_B.b_kstr_ga++) {
13491 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.b_kstr_ga]
13492 = Ttree->data[static_cast<int32_T>(body->Index) - 1]
13493 .f1[cartesian_trajectory_planner_B.b_kstr_ga];
13494 }
13495
13496 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13497 cartesian_trajectory_planner_B.b_kstr_ga < 16;
13498 cartesian_trajectory_planner_B.b_kstr_ga++) {
13499 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_ga]
13500 = body->
13501 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_ga];
13502 }
13503
13504 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13505 cartesian_trajectory_planner_B.b_kstr_ga < 3;
13506 cartesian_trajectory_planner_B.b_kstr_ga++) {
13507 cartesian_trajectory_planner_B.R_c[3 *
13508 cartesian_trajectory_planner_B.b_kstr_ga] =
13509 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_ga];
13510 cartesian_trajectory_planner_B.R_c[3 *
13511 cartesian_trajectory_planner_B.b_kstr_ga + 1] =
13512 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_ga
13513 + 4];
13514 cartesian_trajectory_planner_B.R_c[3 *
13515 cartesian_trajectory_planner_B.b_kstr_ga + 2] =
13516 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_ga
13517 + 8];
13518 }
13519
13520 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13521 cartesian_trajectory_planner_B.b_kstr_ga < 9;
13522 cartesian_trajectory_planner_B.b_kstr_ga++) {
13523 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.b_kstr_ga]
13524 =
13525 -cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr_ga];
13526 }
13527
13528 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13529 cartesian_trajectory_planner_B.b_kstr_ga < 3;
13530 cartesian_trajectory_planner_B.b_kstr_ga++) {
13531 cartesian_trajectory_planner_B.R_ij[cartesian_trajectory_planner_B.b_kstr_ga]
13532 =
13533 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.b_kstr_ga
13534 + 6] * cartesian_trajectory_planner_B.Tdh[14] +
13535 (cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.b_kstr_ga
13536 + 3] * cartesian_trajectory_planner_B.Tdh[13] +
13537 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.b_kstr_ga]
13538 * cartesian_trajectory_planner_B.Tdh[12]);
13539 }
13540
13541 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13542 cartesian_trajectory_planner_B.b_kstr_ga < 4;
13543 cartesian_trajectory_planner_B.b_kstr_ga++) {
13544 for (cartesian_trajectory_planner_B.kstr = 0;
13545 cartesian_trajectory_planner_B.kstr < 4;
13546 cartesian_trajectory_planner_B.kstr++) {
13547 cartesian_trajectory_planner_B.n_j =
13548 cartesian_trajectory_planner_B.kstr << 2;
13549 cartesian_trajectory_planner_B.loop_ub_e =
13550 cartesian_trajectory_planner_B.b_kstr_ga +
13551 cartesian_trajectory_planner_B.n_j;
13552 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_e]
13553 = 0.0;
13554 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_e]
13555 +=
13556 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.n_j]
13557 * cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_ga];
13558 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_e]
13559 +=
13560 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.n_j
13561 + 1] *
13562 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_ga
13563 + 4];
13564 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_e]
13565 +=
13566 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.n_j
13567 + 2] *
13568 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_ga
13569 + 8];
13570 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_e]
13571 +=
13572 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.n_j
13573 + 3] *
13574 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_ga
13575 + 12];
13576 }
13577 }
13578
13579 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13580 cartesian_trajectory_planner_B.b_kstr_ga < 3;
13581 cartesian_trajectory_planner_B.b_kstr_ga++) {
13582 cartesian_trajectory_planner_B.kstr =
13583 cartesian_trajectory_planner_B.b_kstr_ga << 2;
13584 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.kstr]
13585 = cartesian_trajectory_planner_B.R_c[3 *
13586 cartesian_trajectory_planner_B.b_kstr_ga];
13587 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.kstr
13588 + 1] = cartesian_trajectory_planner_B.R_c[3 *
13589 cartesian_trajectory_planner_B.b_kstr_ga + 1];
13590 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.kstr
13591 + 2] = cartesian_trajectory_planner_B.R_c[3 *
13592 cartesian_trajectory_planner_B.b_kstr_ga + 2];
13593 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.b_kstr_ga
13594 + 12] =
13595 cartesian_trajectory_planner_B.R_ij[cartesian_trajectory_planner_B.b_kstr_ga];
13596 }
13597
13598 cartesian_trajectory_planner_B.T1_d[3] = 0.0;
13599 cartesian_trajectory_planner_B.T1_d[7] = 0.0;
13600 cartesian_trajectory_planner_B.T1_d[11] = 0.0;
13601 cartesian_trajectory_planner_B.T1_d[15] = 1.0;
13602 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13603 cartesian_trajectory_planner_B.b_kstr_ga < 4;
13604 cartesian_trajectory_planner_B.b_kstr_ga++) {
13605 for (cartesian_trajectory_planner_B.kstr = 0;
13606 cartesian_trajectory_planner_B.kstr < 4;
13607 cartesian_trajectory_planner_B.kstr++) {
13608 cartesian_trajectory_planner_B.loop_ub_e =
13609 cartesian_trajectory_planner_B.kstr << 2;
13610 cartesian_trajectory_planner_B.n_j =
13611 cartesian_trajectory_planner_B.b_kstr_ga +
13612 cartesian_trajectory_planner_B.loop_ub_e;
13613 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] =
13614 0.0;
13615 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
13616 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.loop_ub_e]
13617 * cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_ga];
13618 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
13619 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.loop_ub_e
13620 + 1] *
13621 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_ga
13622 + 4];
13623 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
13624 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.loop_ub_e
13625 + 2] *
13626 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_ga
13627 + 8];
13628 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
13629 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.loop_ub_e
13630 + 3] *
13631 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_ga
13632 + 12];
13633 }
13634 }
13635
13636 cartesian_trajectory_planner_B.endeffectorIndex = obj->
13637 PositionDoFMap[cartesian_trajectory_planner_B.b_i_j];
13638 cartesian_trajectory_planner_B.idx_idx_1 = obj->
13639 PositionDoFMap[cartesian_trajectory_planner_B.b_i_j + 8];
13640 cartesian_trajectory_planner_B.R_c[0] = 0.0;
13641 cartesian_trajectory_planner_B.R_c[3] = -cartesian_trajectory_planner_B.T
13642 [14];
13643 cartesian_trajectory_planner_B.R_c[6] = cartesian_trajectory_planner_B.T
13644 [13];
13645 cartesian_trajectory_planner_B.R_c[1] = cartesian_trajectory_planner_B.T
13646 [14];
13647 cartesian_trajectory_planner_B.R_c[4] = 0.0;
13648 cartesian_trajectory_planner_B.R_c[7] = -cartesian_trajectory_planner_B.T
13649 [12];
13650 cartesian_trajectory_planner_B.R_c[2] = -cartesian_trajectory_planner_B.T
13651 [13];
13652 cartesian_trajectory_planner_B.R_c[5] = cartesian_trajectory_planner_B.T
13653 [12];
13654 cartesian_trajectory_planner_B.R_c[8] = 0.0;
13655 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13656 cartesian_trajectory_planner_B.b_kstr_ga < 3;
13657 cartesian_trajectory_planner_B.b_kstr_ga++) {
13658 for (cartesian_trajectory_planner_B.kstr = 0;
13659 cartesian_trajectory_planner_B.kstr < 3;
13660 cartesian_trajectory_planner_B.kstr++) {
13661 cartesian_trajectory_planner_B.loop_ub_e =
13662 cartesian_trajectory_planner_B.b_kstr_ga + 3 *
13663 cartesian_trajectory_planner_B.kstr;
13664 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.loop_ub_e]
13665 = 0.0;
13666 cartesian_trajectory_planner_B.n_j =
13667 cartesian_trajectory_planner_B.kstr << 2;
13668 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.loop_ub_e]
13669 +=
13670 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j]
13671 * cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr_ga];
13672 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.loop_ub_e]
13673 +=
13674 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j
13675 + 1] *
13676 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr_ga
13677 + 3];
13678 cartesian_trajectory_planner_B.R_o3[cartesian_trajectory_planner_B.loop_ub_e]
13679 +=
13680 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j
13681 + 2] *
13682 cartesian_trajectory_planner_B.R_c[cartesian_trajectory_planner_B.b_kstr_ga
13683 + 6];
13684 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr
13685 + 6 * cartesian_trajectory_planner_B.b_kstr_ga] =
13686 cartesian_trajectory_planner_B.T
13687 [(cartesian_trajectory_planner_B.b_kstr_ga << 2) +
13688 cartesian_trajectory_planner_B.kstr];
13689 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr
13690 + 6 * (cartesian_trajectory_planner_B.b_kstr_ga + 3)] = 0.0;
13691 }
13692 }
13693
13694 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13695 cartesian_trajectory_planner_B.b_kstr_ga < 3;
13696 cartesian_trajectory_planner_B.b_kstr_ga++) {
13697 cartesian_trajectory_planner_B.X_f[6 *
13698 cartesian_trajectory_planner_B.b_kstr_ga + 3] =
13699 cartesian_trajectory_planner_B.R_o3[3 *
13700 cartesian_trajectory_planner_B.b_kstr_ga];
13701 cartesian_trajectory_planner_B.kstr =
13702 cartesian_trajectory_planner_B.b_kstr_ga << 2;
13703 cartesian_trajectory_planner_B.loop_ub_e = 6 *
13704 (cartesian_trajectory_planner_B.b_kstr_ga + 3);
13705 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_e
13706 + 3] =
13707 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.kstr];
13708 cartesian_trajectory_planner_B.X_f[6 *
13709 cartesian_trajectory_planner_B.b_kstr_ga + 4] =
13710 cartesian_trajectory_planner_B.R_o3[3 *
13711 cartesian_trajectory_planner_B.b_kstr_ga + 1];
13712 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_e
13713 + 4] =
13714 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.kstr +
13715 1];
13716 cartesian_trajectory_planner_B.X_f[6 *
13717 cartesian_trajectory_planner_B.b_kstr_ga + 5] =
13718 cartesian_trajectory_planner_B.R_o3[3 *
13719 cartesian_trajectory_planner_B.b_kstr_ga + 2];
13720 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_e
13721 + 5] =
13722 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.kstr +
13723 2];
13724 }
13725
13726 cartesian_trajectory_planner_B.b_kstr_ga = b->size[0] * b->size[1];
13727 b->size[0] = 6;
13728 b->size[1] = body->JointInternal.MotionSubspace->size[1];
13729 cartes_emxEnsureCapacity_real_T(b,
13730 cartesian_trajectory_planner_B.b_kstr_ga);
13731 cartesian_trajectory_planner_B.loop_ub_e =
13732 body->JointInternal.MotionSubspace->size[0] *
13733 body->JointInternal.MotionSubspace->size[1] - 1;
13734 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13735 cartesian_trajectory_planner_B.b_kstr_ga <=
13736 cartesian_trajectory_planner_B.loop_ub_e;
13737 cartesian_trajectory_planner_B.b_kstr_ga++) {
13738 b->data[cartesian_trajectory_planner_B.b_kstr_ga] =
13739 body->JointInternal.MotionSubspace->
13740 data[cartesian_trajectory_planner_B.b_kstr_ga];
13741 }
13742
13743 cartesian_trajectory_planner_B.n_j = b->size[1] - 1;
13744 cartesian_trajectory_planner_B.b_kstr_ga = JacSlice->size[0] *
13745 JacSlice->size[1];
13746 JacSlice->size[0] = 6;
13747 JacSlice->size[1] = b->size[1];
13748 cartes_emxEnsureCapacity_real_T(JacSlice,
13749 cartesian_trajectory_planner_B.b_kstr_ga);
13750 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13751 cartesian_trajectory_planner_B.b_kstr_ga <=
13752 cartesian_trajectory_planner_B.n_j;
13753 cartesian_trajectory_planner_B.b_kstr_ga++) {
13754 cartesian_trajectory_planner_B.coffset_tmp_j =
13755 cartesian_trajectory_planner_B.b_kstr_ga * 6 - 1;
13756 for (cartesian_trajectory_planner_B.kstr = 0;
13757 cartesian_trajectory_planner_B.kstr < 6;
13758 cartesian_trajectory_planner_B.kstr++) {
13759 cartesian_trajectory_planner_B.s_n = 0.0;
13760 for (cartesian_trajectory_planner_B.loop_ub_e = 0;
13761 cartesian_trajectory_planner_B.loop_ub_e < 6;
13762 cartesian_trajectory_planner_B.loop_ub_e++) {
13763 cartesian_trajectory_planner_B.s_n +=
13764 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_e
13765 * 6 + cartesian_trajectory_planner_B.kstr] * b->data
13766 [(cartesian_trajectory_planner_B.coffset_tmp_j +
13767 cartesian_trajectory_planner_B.loop_ub_e) + 1];
13768 }
13769
13770 JacSlice->data[(cartesian_trajectory_planner_B.coffset_tmp_j +
13771 cartesian_trajectory_planner_B.kstr) + 1] =
13772 cartesian_trajectory_planner_B.s_n;
13773 }
13774 }
13775
13776 if (cartesian_trajectory_planner_B.endeffectorIndex >
13777 cartesian_trajectory_planner_B.idx_idx_1) {
13778 cartesian_trajectory_planner_B.n_j = 0;
13779 } else {
13780 cartesian_trajectory_planner_B.n_j = static_cast<int32_T>
13781 (cartesian_trajectory_planner_B.endeffectorIndex) - 1;
13782 }
13783
13784 cartesian_trajectory_planner_B.loop_ub_e = JacSlice->size[1];
13785 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13786 cartesian_trajectory_planner_B.b_kstr_ga <
13787 cartesian_trajectory_planner_B.loop_ub_e;
13788 cartesian_trajectory_planner_B.b_kstr_ga++) {
13789 for (cartesian_trajectory_planner_B.kstr = 0;
13790 cartesian_trajectory_planner_B.kstr < 6;
13791 cartesian_trajectory_planner_B.kstr++) {
13792 Jac->data[cartesian_trajectory_planner_B.kstr + 6 *
13793 (cartesian_trajectory_planner_B.n_j +
13794 cartesian_trajectory_planner_B.b_kstr_ga)] = JacSlice->data[6 *
13795 cartesian_trajectory_planner_B.b_kstr_ga +
13796 cartesian_trajectory_planner_B.kstr];
13797 }
13798 }
13799 }
13800 }
13801
13802 cartesian_traj_emxFree_char_T_a(&bname);
13803 cartesian_trajec_emxFree_real_T(&JacSlice);
13804 cartesian_emxFree_f_cell_wrap_a(&Ttree);
13805 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13806 cartesian_trajectory_planner_B.b_kstr_ga < 3;
13807 cartesian_trajectory_planner_B.b_kstr_ga++) {
13808 cartesian_trajectory_planner_B.b_i_j =
13809 cartesian_trajectory_planner_B.b_kstr_ga << 2;
13810 cartesian_trajectory_planner_B.X_f[6 *
13811 cartesian_trajectory_planner_B.b_kstr_ga] =
13812 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_j];
13813 cartesian_trajectory_planner_B.kstr = 6 *
13814 (cartesian_trajectory_planner_B.b_kstr_ga + 3);
13815 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr] =
13816 0.0;
13817 cartesian_trajectory_planner_B.X_f[6 *
13818 cartesian_trajectory_planner_B.b_kstr_ga + 3] = 0.0;
13819 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 3] =
13820 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_j];
13821 cartesian_trajectory_planner_B.endeffectorIndex =
13822 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_j + 1];
13823 cartesian_trajectory_planner_B.X_f[6 *
13824 cartesian_trajectory_planner_B.b_kstr_ga + 1] =
13825 cartesian_trajectory_planner_B.endeffectorIndex;
13826 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 1] =
13827 0.0;
13828 cartesian_trajectory_planner_B.X_f[6 *
13829 cartesian_trajectory_planner_B.b_kstr_ga + 4] = 0.0;
13830 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 4] =
13831 cartesian_trajectory_planner_B.endeffectorIndex;
13832 cartesian_trajectory_planner_B.endeffectorIndex =
13833 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_j + 2];
13834 cartesian_trajectory_planner_B.X_f[6 *
13835 cartesian_trajectory_planner_B.b_kstr_ga + 2] =
13836 cartesian_trajectory_planner_B.endeffectorIndex;
13837 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 2] =
13838 0.0;
13839 cartesian_trajectory_planner_B.X_f[6 *
13840 cartesian_trajectory_planner_B.b_kstr_ga + 5] = 0.0;
13841 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 5] =
13842 cartesian_trajectory_planner_B.endeffectorIndex;
13843 }
13844
13845 cartesian_trajectory_planner_B.n_j = Jac->size[1];
13846 cartesian_trajectory_planner_B.b_kstr_ga = b->size[0] * b->size[1];
13847 b->size[0] = 6;
13848 b->size[1] = Jac->size[1];
13849 cartes_emxEnsureCapacity_real_T(b, cartesian_trajectory_planner_B.b_kstr_ga);
13850 cartesian_trajectory_planner_B.loop_ub_e = Jac->size[0] * Jac->size[1] - 1;
13851 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13852 cartesian_trajectory_planner_B.b_kstr_ga <=
13853 cartesian_trajectory_planner_B.loop_ub_e;
13854 cartesian_trajectory_planner_B.b_kstr_ga++) {
13855 b->data[cartesian_trajectory_planner_B.b_kstr_ga] = Jac->
13856 data[cartesian_trajectory_planner_B.b_kstr_ga];
13857 }
13858
13859 cartesian_trajectory_planner_B.b_kstr_ga = Jac->size[0] * Jac->size[1];
13860 Jac->size[0] = 6;
13861 Jac->size[1] = cartesian_trajectory_planner_B.n_j;
13862 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr_ga);
13863 for (cartesian_trajectory_planner_B.b_kstr_ga = 0;
13864 cartesian_trajectory_planner_B.b_kstr_ga <
13865 cartesian_trajectory_planner_B.n_j;
13866 cartesian_trajectory_planner_B.b_kstr_ga++) {
13867 cartesian_trajectory_planner_B.coffset_tmp_j =
13868 cartesian_trajectory_planner_B.b_kstr_ga * 6 - 1;
13869 for (cartesian_trajectory_planner_B.b_i_j = 0;
13870 cartesian_trajectory_planner_B.b_i_j < 6;
13871 cartesian_trajectory_planner_B.b_i_j++) {
13872 cartesian_trajectory_planner_B.s_n = 0.0;
13873 for (cartesian_trajectory_planner_B.loop_ub_e = 0;
13874 cartesian_trajectory_planner_B.loop_ub_e < 6;
13875 cartesian_trajectory_planner_B.loop_ub_e++) {
13876 cartesian_trajectory_planner_B.s_n +=
13877 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_e
13878 * 6 + cartesian_trajectory_planner_B.b_i_j] * b->data
13879 [(cartesian_trajectory_planner_B.coffset_tmp_j +
13880 cartesian_trajectory_planner_B.loop_ub_e) + 1];
13881 }
13882
13883 Jac->data[(cartesian_trajectory_planner_B.coffset_tmp_j +
13884 cartesian_trajectory_planner_B.b_i_j) + 1] =
13885 cartesian_trajectory_planner_B.s_n;
13886 }
13887 }
13888
13889 cartesian_trajec_emxFree_real_T(&b);
13890}
13891
13892void rt_invd6x6_snf(const real_T u[36], real_T y[36])
13893{
13894 int8_T p[6];
13895 real_T A[36];
13896 int8_T ipiv[6];
13897 int32_T jj;
13898 int32_T j;
13899 int32_T kAcol;
13900 int32_T c;
13901 int32_T ix;
13902 real_T smax;
13903 real_T s;
13904 int32_T iy;
13905 int32_T jy;
13906 int32_T j_0;
13907 int32_T ijA;
13908 for (iy = 0; iy < 36; iy++) {
13909 y[iy] = 0.0;
13910 A[iy] = u[iy];
13911 }
13912
13913 for (iy = 0; iy < 6; iy++) {
13914 ipiv[iy] = static_cast<int8_T>(iy + 1);
13915 }
13916
13917 for (j = 0; j < 5; j++) {
13918 c = j * 7 + 2;
13919 jj = j * 7;
13920 kAcol = 6 - j;
13921 iy = 1;
13922 ix = c - 2;
13923 smax = fabs(A[jj]);
13924 for (jy = 2; jy <= kAcol; jy++) {
13925 ix++;
13926 s = fabs(A[ix]);
13927 if (s > smax) {
13928 iy = jy;
13929 smax = s;
13930 }
13931 }
13932
13933 if (A[(c + iy) - 3] != 0.0) {
13934 if (iy - 1 != 0) {
13935 jy = j + iy;
13936 ipiv[j] = static_cast<int8_T>(jy);
13937 ix = j;
13938 iy = jy - 1;
13939 for (jy = 0; jy < 6; jy++) {
13940 smax = A[ix];
13941 A[ix] = A[iy];
13942 A[iy] = smax;
13943 ix += 6;
13944 iy += 6;
13945 }
13946 }
13947
13948 iy = c - j;
13949 for (ix = c; ix <= iy + 4; ix++) {
13950 A[ix - 1] /= A[jj];
13951 }
13952 }
13953
13954 kAcol = 4 - j;
13955 jy = jj + 6;
13956 for (j_0 = 0; j_0 <= kAcol; j_0++) {
13957 if (A[jy] != 0.0) {
13958 smax = -A[jy];
13959 ix = c - 1;
13960 iy = jj - j;
13961 for (ijA = jj + 8; ijA <= iy + 12; ijA++) {
13962 A[ijA - 1] += A[ix] * smax;
13963 ix++;
13964 }
13965 }
13966
13967 jy += 6;
13968 jj += 6;
13969 }
13970 }
13971
13972 for (iy = 0; iy < 6; iy++) {
13973 p[iy] = static_cast<int8_T>(iy + 1);
13974 }
13975
13976 for (jy = 0; jy < 5; jy++) {
13977 if (ipiv[jy] > jy + 1) {
13978 j = ipiv[jy] - 1;
13979 iy = p[j];
13980 p[j] = p[jy];
13981 p[jy] = static_cast<int8_T>(iy);
13982 }
13983 }
13984
13985 for (jy = 0; jy < 6; jy++) {
13986 jj = p[jy] - 1;
13987 y[jy + 6 * jj] = 1.0;
13988 for (j = jy + 1; j < 7; j++) {
13989 iy = (6 * jj + j) - 1;
13990 if (y[iy] != 0.0) {
13991 for (ix = j + 1; ix < 7; ix++) {
13992 c = (6 * jj + ix) - 1;
13993 y[c] -= A[((j - 1) * 6 + ix) - 1] * y[iy];
13994 }
13995 }
13996 }
13997 }
13998
13999 for (j = 0; j < 6; j++) {
14000 jj = 6 * j;
14001 for (jy = 5; jy >= 0; jy--) {
14002 kAcol = 6 * jy;
14003 iy = jy + jj;
14004 if (y[iy] != 0.0) {
14005 y[iy] /= A[jy + kAcol];
14006 j_0 = jy - 1;
14007 for (ix = 0; ix <= j_0; ix++) {
14008 c = ix + jj;
14009 y[c] -= y[iy] * A[ix + kAcol];
14010 }
14011 }
14012 }
14013 }
14014}
14015
14016static void matlabCodegenHandle_matla_astwh(ros_slros_internal_block_Subs_T *obj)
14017{
14018 if (!obj->matlabCodegenIsDeleted) {
14019 obj->matlabCodegenIsDeleted = true;
14020 }
14021}
14022
14023static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj)
14024{
14025 if (!obj->matlabCodegenIsDeleted) {
14026 obj->matlabCodegenIsDeleted = true;
14027 }
14028}
14029
14030static void matlabCodegenHandle_matlabC_ast(robotics_slmanip_internal__as_T *obj)
14031{
14032 if (!obj->matlabCodegenIsDeleted) {
14033 obj->matlabCodegenIsDeleted = true;
14034 }
14035}
14036
14037static void cartesian_tr_SystemCore_release(b_inverseKinematics_cartesian_T *obj)
14038{
14039 if (obj->isInitialized == 1) {
14040 obj->isInitialized = 2;
14041 }
14042}
14043
14044static void cartesian__SystemCore_delete_as(b_inverseKinematics_cartesian_T *obj)
14045{
14046 cartesian_tr_SystemCore_release(obj);
14047}
14048
14049static void matlabCodegenHandle_matlabCo_as(b_inverseKinematics_cartesian_T *obj)
14050{
14051 if (!obj->matlabCodegenIsDeleted) {
14052 obj->matlabCodegenIsDeleted = true;
14053 cartesian__SystemCore_delete_as(obj);
14054 }
14055}
14056
14057static void emxFreeStruct_c_rigidBodyJoin_a(c_rigidBodyJoint_cartesian_as_T
14058 *pStruct)
14059{
14060 cartesian_traj_emxFree_char_T_a(&pStruct->Type);
14061 cartesian_trajec_emxFree_real_T(&pStruct->MotionSubspace);
14062 cartesian_traj_emxFree_char_T_a(&pStruct->NameInternal);
14063 cartesian_trajec_emxFree_real_T(&pStruct->PositionLimitsInternal);
14064 cartesian_trajec_emxFree_real_T(&pStruct->HomePositionInternal);
14065}
14066
14067static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
14068 *pStruct)
14069{
14070 cartesian_traj_emxFree_char_T_a(&pStruct->NameInternal);
14071 emxFreeStruct_c_rigidBodyJoin_a(&pStruct->JointInternal);
14072}
14073
14074static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
14075 *pStruct)
14076{
14077 emxFreeStruct_v_robotics_manip_(&pStruct->Base);
14078}
14079
14080static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
14081 *pStruct)
14082{
14083 cartesian_trajec_emxFree_real_T(&pStruct->Limits);
14084}
14085
14086static void emxFreeStruct_robotics_slmani_a(robotics_slmanip_internal__as_T
14087 *pStruct)
14088{
14089 emxFreeStruct_y_robotics_manip_(&pStruct->TreeInternal);
14090 emxFreeStruct_b_inverseKinemati(&pStruct->IKInternal);
14091}
14092
14093static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
14094 *pStruct)
14095{
14096 cartesian_traj_emxFree_char_T_a(&pStruct->NameInternal);
14097}
14098
14099static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
14100 *pStruct)
14101{
14102 emxFreeStruct_w_robotics_manip_(&pStruct->Base);
14103}
14104
14105static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
14106 *pStruct)
14107{
14108 cartesian_traj_emxFree_char_T_a(&pStruct->BodyName);
14109 cartesian_trajec_emxFree_real_T(&pStruct->ErrTemp);
14110 cartesian_trajec_emxFree_real_T(&pStruct->GradTemp);
14111}
14112
14113static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
14114 *pStruct)
14115{
14116 cartesian_trajec_emxFree_real_T(&pStruct->ConstraintMatrix);
14117 cartesian_trajec_emxFree_real_T(&pStruct->ConstraintBound);
14118}
14119
14120static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__a_T
14121 *pStruct)
14122{
14123 cartesian_traj_emxFree_char_T_a(&pStruct->Type);
14124 cartesian_trajec_emxFree_real_T(&pStruct->MotionSubspace);
14125}
14126
14127static void emxFreeStruct_o_robotics_mani_a(o_robotics_manip_internal_R_a_T
14128 *pStruct)
14129{
14130 cartesian_traj_emxFree_char_T_a(&pStruct->NameInternal);
14131 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
14132}
14133
14134static void emxFreeStruct_p_robotics_mani_a(p_robotics_manip_internal_R_a_T
14135 *pStruct)
14136{
14137 emxFreeStruct_o_robotics_mani_a(&pStruct->Base);
14138}
14139
14140static void emxFreeStruct_robotics_slman_as(robotics_slmanip_internal_b_a_T
14141 *pStruct)
14142{
14143 emxFreeStruct_p_robotics_mani_a(&pStruct->TreeInternal);
14144}
14145
14146static void emxFreeStruct_n_robotics_mani_a(n_robotics_manip_internal_R_a_T
14147 *pStruct)
14148{
14149 cartesian_traj_emxFree_char_T_a(&pStruct->NameInternal);
14150 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
14151}
14152
14153static void matlabCodegenHandle_matlab_astw(ros_slros_internal_block_Publ_T *obj)
14154{
14155 if (!obj->matlabCodegenIsDeleted) {
14156 obj->matlabCodegenIsDeleted = true;
14157 }
14158}
14159
14160static void emxInitStruct_c_rigidBodyJoin_a(c_rigidBodyJoint_cartesian_as_T
14161 *pStruct)
14162{
14163 cartesian_traj_emxInit_char_T_a(&pStruct->Type, 2);
14164 cartesian_trajec_emxInit_real_T(&pStruct->MotionSubspace, 2);
14165 cartesian_traj_emxInit_char_T_a(&pStruct->NameInternal, 2);
14166 cartesian_trajec_emxInit_real_T(&pStruct->PositionLimitsInternal, 2);
14167 cartesian_trajec_emxInit_real_T(&pStruct->HomePositionInternal, 1);
14168}
14169
14170static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
14171 *pStruct)
14172{
14173 cartesian_traj_emxInit_char_T_a(&pStruct->NameInternal, 2);
14174 emxInitStruct_c_rigidBodyJoin_a(&pStruct->JointInternal);
14175}
14176
14177static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
14178 *pStruct)
14179{
14180 emxInitStruct_v_robotics_manip_(&pStruct->Base);
14181}
14182
14183static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
14184 *pStruct)
14185{
14186 cartesian_trajec_emxInit_real_T(&pStruct->Limits, 2);
14187}
14188
14189static void emxInitStruct_robotics_slmani_a(robotics_slmanip_internal__as_T
14190 *pStruct)
14191{
14192 emxInitStruct_y_robotics_manip_(&pStruct->TreeInternal);
14193 emxInitStruct_b_inverseKinemati(&pStruct->IKInternal);
14194}
14195
14196static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
14197 *pStruct)
14198{
14199 cartesian_traj_emxInit_char_T_a(&pStruct->NameInternal, 2);
14200}
14201
14202static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
14203 *pStruct)
14204{
14205 emxInitStruct_w_robotics_manip_(&pStruct->Base);
14206}
14207
14208static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
14209 *pStruct)
14210{
14211 cartesian_traj_emxInit_char_T_a(&pStruct->BodyName, 2);
14212 cartesian_trajec_emxInit_real_T(&pStruct->ErrTemp, 1);
14213 cartesian_trajec_emxInit_real_T(&pStruct->GradTemp, 1);
14214}
14215
14216static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
14217 *pStruct)
14218{
14219 cartesian_trajec_emxInit_real_T(&pStruct->ConstraintMatrix, 2);
14220 cartesian_trajec_emxInit_real_T(&pStruct->ConstraintBound, 1);
14221}
14222
14223static void cartesia_twister_state_vector_a(uint32_T mt[625])
14224{
14225 uint32_T r;
14226 int32_T b_mti;
14227 r = 5489U;
14228 mt[0] = 5489U;
14229 for (b_mti = 0; b_mti < 623; b_mti++) {
14230 r = ((r >> 30U ^ r) * 1812433253U + b_mti) + 1U;
14231 mt[b_mti + 1] = r;
14232 }
14233
14234 mt[624] = 624U;
14235}
14236
14237static void cartesian_tr_eml_rand_mt19937ar(uint32_T state[625])
14238{
14239 memset(&state[0], 0, 625U * sizeof(uint32_T));
14240 cartesia_twister_state_vector_a(state);
14241}
14242
14243static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_astwhqf2az
14244 (v_robotics_manip_internal_Rig_T *obj)
14245{
14246 v_robotics_manip_internal_Rig_T *b_obj;
14247 int8_T msubspace_data[36];
14248 real_T poslim_data[12];
14249 emxArray_char_T_cartesian_tra_T *switch_expression;
14250 boolean_T b_bool;
14251 int32_T b_kstr;
14252 char_T b[8];
14253 char_T b_0[9];
14254 int32_T loop_ub;
14255 int8_T tmp[6];
14256 static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
14257 'l', 'i', 'n', 'k' };
14258
14259 static const real_T tmp_1[9] = { 0.012583419040406959, -0.00021487638648447484,
14260 -0.00022605919127205462, -0.00021487638648447484, 0.00052369449451288713,
14261 -0.00011525315957400814, -0.00022605919127205462, -0.00011525315957400814,
14262 0.012646079447789898 };
14263
14264 static const real_T tmp_2[36] = { 0.012583419040406959,
14265 -0.00021487638648447484, -0.00022605919127205462, 0.0, -0.00392971169381184,
14266 0.00047022930128152475, -0.00021487638648447484, 0.00052369449451288713,
14267 -0.00011525315957400814, 0.00392971169381184, 0.0, -0.00449464704691423,
14268 -0.00022605919127205462, -0.00011525315957400814, 0.012646079447789898,
14269 -0.00047022930128152475, 0.00449464704691423, 0.0, 0.0, 0.00392971169381184,
14270 -0.00047022930128152475, 0.0785942338762368, 0.0, 0.0, -0.00392971169381184,
14271 0.0, 0.00449464704691423, 0.0, 0.0785942338762368, 0.0,
14272 0.00047022930128152475, -0.00449464704691423, 0.0, 0.0, 0.0,
14273 0.0785942338762368 };
14274
14275 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14276 1 };
14277
14278 static const char_T tmp_4[15] = { 'w', 'o', 'r', 'l', 'd', '_', 'e', 'd', 'o',
14279 '_', 'j', 'o', 'i', 'n', 't' };
14280
14281 static const char_T tmp_5[5] = { 'f', 'i', 'x', 'e', 'd' };
14282
14283 static const char_T tmp_6[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14284
14285 static const char_T tmp_7[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14286
14287 static const real_T tmp_8[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14288 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14289
14290 static const real_T tmp_9[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14291 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14292
14293 int32_T exitg1;
14294 b_obj = obj;
14295 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14296 obj->NameInternal->size[0] = 1;
14297 obj->NameInternal->size[1] = 13;
14298 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
14299 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
14300 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14301 }
14302
14303 obj->ParentIndex = 0.0;
14304 obj->MassInternal = 0.0785942338762368;
14305 obj->CenterOfMassInternal[0] = 0.057188;
14306 obj->CenterOfMassInternal[1] = 0.005983;
14307 obj->CenterOfMassInternal[2] = 0.05;
14308 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14309 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
14310 }
14311
14312 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14313 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
14314 }
14315
14316 obj->JointInternal.InTree = false;
14317 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14318 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
14319 }
14320
14321 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14322 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
14323 }
14324
14325 b_kstr = obj->JointInternal.NameInternal->size[0] *
14326 obj->JointInternal.NameInternal->size[1];
14327 obj->JointInternal.NameInternal->size[0] = 1;
14328 obj->JointInternal.NameInternal->size[1] = 15;
14329 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.NameInternal, b_kstr);
14330 for (b_kstr = 0; b_kstr < 15; b_kstr++) {
14331 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
14332 }
14333
14334 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
14335 obj->JointInternal.Type->size[0] = 1;
14336 obj->JointInternal.Type->size[1] = 5;
14337 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
14338 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
14339 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
14340 }
14341
14342 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
14343 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14344 switch_expression->size[0] = 1;
14345 switch_expression->size[1] = obj->JointInternal.Type->size[1];
14346 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
14347 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
14348 - 1;
14349 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14350 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
14351 }
14352
14353 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14354 b[b_kstr] = tmp_6[b_kstr];
14355 }
14356
14357 b_bool = false;
14358 if (switch_expression->size[1] == 8) {
14359 b_kstr = 1;
14360 do {
14361 exitg1 = 0;
14362 if (b_kstr - 1 < 8) {
14363 loop_ub = b_kstr - 1;
14364 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14365 exitg1 = 1;
14366 } else {
14367 b_kstr++;
14368 }
14369 } else {
14370 b_bool = true;
14371 exitg1 = 1;
14372 }
14373 } while (exitg1 == 0);
14374 }
14375
14376 if (b_bool) {
14377 b_kstr = 0;
14378 } else {
14379 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14380 b_0[b_kstr] = tmp_7[b_kstr];
14381 }
14382
14383 b_bool = false;
14384 if (switch_expression->size[1] == 9) {
14385 b_kstr = 1;
14386 do {
14387 exitg1 = 0;
14388 if (b_kstr - 1 < 9) {
14389 loop_ub = b_kstr - 1;
14390 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14391 exitg1 = 1;
14392 } else {
14393 b_kstr++;
14394 }
14395 } else {
14396 b_bool = true;
14397 exitg1 = 1;
14398 }
14399 } while (exitg1 == 0);
14400 }
14401
14402 if (b_bool) {
14403 b_kstr = 1;
14404 } else {
14405 b_kstr = -1;
14406 }
14407 }
14408
14409 cartesian_traj_emxFree_char_T_a(&switch_expression);
14410 switch (b_kstr) {
14411 case 0:
14412 tmp[0] = 0;
14413 tmp[1] = 0;
14414 tmp[2] = 1;
14415 tmp[3] = 0;
14416 tmp[4] = 0;
14417 tmp[5] = 0;
14418 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14419 msubspace_data[b_kstr] = tmp[b_kstr];
14420 }
14421
14422 poslim_data[0] = -3.1415926535897931;
14423 poslim_data[1] = 3.1415926535897931;
14424 obj->JointInternal.VelocityNumber = 1.0;
14425 obj->JointInternal.PositionNumber = 1.0;
14426 obj->JointInternal.JointAxisInternal[0] = 0.0;
14427 obj->JointInternal.JointAxisInternal[1] = 0.0;
14428 obj->JointInternal.JointAxisInternal[2] = 1.0;
14429 break;
14430
14431 case 1:
14432 tmp[0] = 0;
14433 tmp[1] = 0;
14434 tmp[2] = 0;
14435 tmp[3] = 0;
14436 tmp[4] = 0;
14437 tmp[5] = 1;
14438 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14439 msubspace_data[b_kstr] = tmp[b_kstr];
14440 }
14441
14442 poslim_data[0] = -0.5;
14443 poslim_data[1] = 0.5;
14444 obj->JointInternal.VelocityNumber = 1.0;
14445 obj->JointInternal.PositionNumber = 1.0;
14446 obj->JointInternal.JointAxisInternal[0] = 0.0;
14447 obj->JointInternal.JointAxisInternal[1] = 0.0;
14448 obj->JointInternal.JointAxisInternal[2] = 1.0;
14449 break;
14450
14451 default:
14452 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14453 msubspace_data[b_kstr] = 0;
14454 }
14455
14456 poslim_data[0] = 0.0;
14457 poslim_data[1] = 0.0;
14458 obj->JointInternal.VelocityNumber = 0.0;
14459 obj->JointInternal.PositionNumber = 0.0;
14460 obj->JointInternal.JointAxisInternal[0] = 0.0;
14461 obj->JointInternal.JointAxisInternal[1] = 0.0;
14462 obj->JointInternal.JointAxisInternal[2] = 0.0;
14463 break;
14464 }
14465
14466 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14467 obj->JointInternal.MotionSubspace->size[1];
14468 obj->JointInternal.MotionSubspace->size[0] = 6;
14469 obj->JointInternal.MotionSubspace->size[1] = 1;
14470 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14471 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14472 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14473 }
14474
14475 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14476 obj->JointInternal.PositionLimitsInternal->size[1];
14477 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14478 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14479 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14480 b_kstr);
14481 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14482 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14483 }
14484
14485 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14486 obj->JointInternal.HomePositionInternal->size[0] = 1;
14487 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14488 b_kstr);
14489 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14490 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14491 }
14492
14493 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14494 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_8[b_kstr];
14495 }
14496
14497 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14498 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_9[b_kstr];
14499 }
14500
14501 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14502 obj->JointInternal.MotionSubspace->size[1];
14503 obj->JointInternal.MotionSubspace->size[0] = 6;
14504 obj->JointInternal.MotionSubspace->size[1] = 1;
14505 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14506 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14507 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
14508 }
14509
14510 obj->JointInternal.InTree = true;
14511 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14512 obj->JointInternal.PositionLimitsInternal->size[1];
14513 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14514 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14515 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14516 b_kstr);
14517 obj->JointInternal.PositionLimitsInternal->data[0] = 0.0;
14518 obj->JointInternal.PositionLimitsInternal->data
14519 [obj->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
14520 obj->JointInternal.JointAxisInternal[0] = 0.0;
14521 obj->JointInternal.JointAxisInternal[1] = 0.0;
14522 obj->JointInternal.JointAxisInternal[2] = 0.0;
14523 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14524 obj->JointInternal.HomePositionInternal->size[0] = 1;
14525 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14526 b_kstr);
14527 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14528 return b_obj;
14529}
14530
14531static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_astwhqf2azt
14532 (v_robotics_manip_internal_Rig_T *obj)
14533{
14534 v_robotics_manip_internal_Rig_T *b_obj;
14535 int8_T msubspace_data[36];
14536 real_T poslim_data[12];
14537 emxArray_char_T_cartesian_tra_T *switch_expression;
14538 boolean_T b_bool;
14539 int32_T b_kstr;
14540 char_T b[8];
14541 char_T b_0[9];
14542 int32_T loop_ub;
14543 int8_T tmp[6];
14544 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
14545 '1' };
14546
14547 static const real_T tmp_1[9] = { 0.012559660892485551, 0.00032713982710414,
14548 -1.32683892634271E-6, 0.00032713982710414, 0.00018048007331848145,
14549 9.17416945099368E-5, -1.32683892634271E-6, 9.17416945099368E-5,
14550 0.012672078048055372 };
14551
14552 static const real_T tmp_2[36] = { 0.012559660892485551, 0.00032713982710414,
14553 -1.32683892634271E-6, 0.0, -0.0, 0.0037143634929909515, 0.00032713982710414,
14554 0.00018048007331848145, 9.17416945099368E-5, 0.0, 0.0, 0.0029444543779393356,
14555 -1.32683892634271E-6, 9.17416945099368E-5, 0.012672078048055372,
14556 -0.0037143634929909515, -0.0029444543779393356, 0.0, 0.0, 0.0,
14557 -0.0037143634929909515, 0.0785942338762368, 0.0, 0.0, -0.0, 0.0,
14558 -0.0029444543779393356, 0.0, 0.0785942338762368, 0.0, 0.0037143634929909515,
14559 0.0029444543779393356, 0.0, 0.0, 0.0, 0.0785942338762368 };
14560
14561 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14562 1 };
14563
14564 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
14565 '_', '1' };
14566
14567 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14568
14569 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14570
14571 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14572 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
14573
14574 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14575 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14576
14577 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14578 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14579 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
14580
14581 int32_T exitg1;
14582 b_obj = obj;
14583 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14584 obj->NameInternal->size[0] = 1;
14585 obj->NameInternal->size[1] = 10;
14586 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
14587 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14588 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14589 }
14590
14591 obj->ParentIndex = 1.0;
14592 obj->MassInternal = 0.0785942338762368;
14593 obj->CenterOfMassInternal[0] = -0.037464;
14594 obj->CenterOfMassInternal[1] = 0.04726;
14595 obj->CenterOfMassInternal[2] = 0.0;
14596 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14597 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
14598 }
14599
14600 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14601 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
14602 }
14603
14604 obj->JointInternal.InTree = false;
14605 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14606 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
14607 }
14608
14609 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14610 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
14611 }
14612
14613 b_kstr = obj->JointInternal.NameInternal->size[0] *
14614 obj->JointInternal.NameInternal->size[1];
14615 obj->JointInternal.NameInternal->size[0] = 1;
14616 obj->JointInternal.NameInternal->size[1] = 11;
14617 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.NameInternal, b_kstr);
14618 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
14619 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
14620 }
14621
14622 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
14623 obj->JointInternal.Type->size[0] = 1;
14624 obj->JointInternal.Type->size[1] = 8;
14625 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
14626 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14627 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
14628 }
14629
14630 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
14631 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14632 switch_expression->size[0] = 1;
14633 switch_expression->size[1] = obj->JointInternal.Type->size[1];
14634 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
14635 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
14636 - 1;
14637 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14638 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
14639 }
14640
14641 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14642 b[b_kstr] = tmp_5[b_kstr];
14643 }
14644
14645 b_bool = false;
14646 if (switch_expression->size[1] == 8) {
14647 b_kstr = 1;
14648 do {
14649 exitg1 = 0;
14650 if (b_kstr - 1 < 8) {
14651 loop_ub = b_kstr - 1;
14652 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14653 exitg1 = 1;
14654 } else {
14655 b_kstr++;
14656 }
14657 } else {
14658 b_bool = true;
14659 exitg1 = 1;
14660 }
14661 } while (exitg1 == 0);
14662 }
14663
14664 if (b_bool) {
14665 b_kstr = 0;
14666 } else {
14667 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14668 b_0[b_kstr] = tmp_6[b_kstr];
14669 }
14670
14671 b_bool = false;
14672 if (switch_expression->size[1] == 9) {
14673 b_kstr = 1;
14674 do {
14675 exitg1 = 0;
14676 if (b_kstr - 1 < 9) {
14677 loop_ub = b_kstr - 1;
14678 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14679 exitg1 = 1;
14680 } else {
14681 b_kstr++;
14682 }
14683 } else {
14684 b_bool = true;
14685 exitg1 = 1;
14686 }
14687 } while (exitg1 == 0);
14688 }
14689
14690 if (b_bool) {
14691 b_kstr = 1;
14692 } else {
14693 b_kstr = -1;
14694 }
14695 }
14696
14697 cartesian_traj_emxFree_char_T_a(&switch_expression);
14698 switch (b_kstr) {
14699 case 0:
14700 tmp[0] = 0;
14701 tmp[1] = 0;
14702 tmp[2] = 1;
14703 tmp[3] = 0;
14704 tmp[4] = 0;
14705 tmp[5] = 0;
14706 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14707 msubspace_data[b_kstr] = tmp[b_kstr];
14708 }
14709
14710 poslim_data[0] = -3.1415926535897931;
14711 poslim_data[1] = 3.1415926535897931;
14712 obj->JointInternal.VelocityNumber = 1.0;
14713 obj->JointInternal.PositionNumber = 1.0;
14714 obj->JointInternal.JointAxisInternal[0] = 0.0;
14715 obj->JointInternal.JointAxisInternal[1] = 0.0;
14716 obj->JointInternal.JointAxisInternal[2] = 1.0;
14717 break;
14718
14719 case 1:
14720 tmp[0] = 0;
14721 tmp[1] = 0;
14722 tmp[2] = 0;
14723 tmp[3] = 0;
14724 tmp[4] = 0;
14725 tmp[5] = 1;
14726 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14727 msubspace_data[b_kstr] = tmp[b_kstr];
14728 }
14729
14730 poslim_data[0] = -0.5;
14731 poslim_data[1] = 0.5;
14732 obj->JointInternal.VelocityNumber = 1.0;
14733 obj->JointInternal.PositionNumber = 1.0;
14734 obj->JointInternal.JointAxisInternal[0] = 0.0;
14735 obj->JointInternal.JointAxisInternal[1] = 0.0;
14736 obj->JointInternal.JointAxisInternal[2] = 1.0;
14737 break;
14738
14739 default:
14740 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14741 msubspace_data[b_kstr] = 0;
14742 }
14743
14744 poslim_data[0] = 0.0;
14745 poslim_data[1] = 0.0;
14746 obj->JointInternal.VelocityNumber = 0.0;
14747 obj->JointInternal.PositionNumber = 0.0;
14748 obj->JointInternal.JointAxisInternal[0] = 0.0;
14749 obj->JointInternal.JointAxisInternal[1] = 0.0;
14750 obj->JointInternal.JointAxisInternal[2] = 0.0;
14751 break;
14752 }
14753
14754 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14755 obj->JointInternal.MotionSubspace->size[1];
14756 obj->JointInternal.MotionSubspace->size[0] = 6;
14757 obj->JointInternal.MotionSubspace->size[1] = 1;
14758 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14759 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14760 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14761 }
14762
14763 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14764 obj->JointInternal.PositionLimitsInternal->size[1];
14765 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14766 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14767 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14768 b_kstr);
14769 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14770 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14771 }
14772
14773 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14774 obj->JointInternal.HomePositionInternal->size[0] = 1;
14775 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14776 b_kstr);
14777 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14778 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14779 }
14780
14781 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14782 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
14783 }
14784
14785 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14786 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
14787 }
14788
14789 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14790 obj->JointInternal.MotionSubspace->size[1];
14791 obj->JointInternal.MotionSubspace->size[0] = 6;
14792 obj->JointInternal.MotionSubspace->size[1] = 1;
14793 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14794 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14795 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
14796 }
14797
14798 obj->JointInternal.InTree = true;
14799 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14800 obj->JointInternal.PositionLimitsInternal->size[1];
14801 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14802 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14803 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14804 b_kstr);
14805 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
14806 obj->JointInternal.PositionLimitsInternal->data
14807 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
14808 obj->JointInternal.JointAxisInternal[0] = 0.0;
14809 obj->JointInternal.JointAxisInternal[1] = 0.0;
14810 obj->JointInternal.JointAxisInternal[2] = 1.0;
14811 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14812 obj->JointInternal.HomePositionInternal->size[0] = 1;
14813 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14814 b_kstr);
14815 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14816 return b_obj;
14817}
14818
14819static v_robotics_manip_internal_Rig_T *RigidBody_RigidBod_astwhqf2aztt
14820 (v_robotics_manip_internal_Rig_T *obj)
14821{
14822 v_robotics_manip_internal_Rig_T *b_obj;
14823 int8_T msubspace_data[36];
14824 real_T poslim_data[12];
14825 emxArray_char_T_cartesian_tra_T *switch_expression;
14826 boolean_T b_bool;
14827 int32_T b_kstr;
14828 char_T b[8];
14829 char_T b_0[9];
14830 int32_T loop_ub;
14831 int8_T tmp[6];
14832 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
14833 '2' };
14834
14835 static const real_T tmp_1[9] = { 0.012452446533447339, 0.00097164648860403489,
14836 -0.0012079400901817208, 0.00097164648860403489, 0.0077631932790618,
14837 0.0060529024261622953, -0.0012079400901817208, 0.0060529024261622953,
14838 0.0051581160550583753 };
14839
14840 static const real_T tmp_2[36] = { 0.012452446533447339, 0.00097164648860403489,
14841 -0.0012079400901817208, 0.0, 0.0036112478581453288, 0.0024995324199659588,
14842 0.00097164648860403489, 0.0077631932790618, 0.0060529024261622953,
14843 -0.0036112478581453288, 0.0, 0.0012907531029494371, -0.0012079400901817208,
14844 0.0060529024261622953, 0.0051581160550583753, -0.0024995324199659588,
14845 -0.0012907531029494371, 0.0, 0.0, -0.0036112478581453288,
14846 -0.0024995324199659588, 0.0785942338762368, 0.0, 0.0, 0.0036112478581453288,
14847 0.0, -0.0012907531029494371, 0.0, 0.0785942338762368, 0.0,
14848 0.0024995324199659588, 0.0012907531029494371, 0.0, 0.0, 0.0,
14849 0.0785942338762368 };
14850
14851 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14852 1 };
14853
14854 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
14855 '_', '2' };
14856
14857 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14858
14859 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14860
14861 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
14862 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
14863 0.0, 0.0, 0.0, 1.0 };
14864
14865 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14866 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14867
14868 static const real_T tmp_9[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14869 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14870 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
14871
14872 int32_T exitg1;
14873 b_obj = obj;
14874 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14875 obj->NameInternal->size[0] = 1;
14876 obj->NameInternal->size[1] = 10;
14877 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
14878 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14879 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14880 }
14881
14882 obj->ParentIndex = 2.0;
14883 obj->MassInternal = 0.0785942338762368;
14884 obj->CenterOfMassInternal[0] = -0.016423;
14885 obj->CenterOfMassInternal[1] = 0.031803;
14886 obj->CenterOfMassInternal[2] = -0.045948;
14887 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14888 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
14889 }
14890
14891 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14892 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
14893 }
14894
14895 obj->JointInternal.InTree = false;
14896 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14897 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
14898 }
14899
14900 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14901 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
14902 }
14903
14904 b_kstr = obj->JointInternal.NameInternal->size[0] *
14905 obj->JointInternal.NameInternal->size[1];
14906 obj->JointInternal.NameInternal->size[0] = 1;
14907 obj->JointInternal.NameInternal->size[1] = 11;
14908 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.NameInternal, b_kstr);
14909 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
14910 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
14911 }
14912
14913 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
14914 obj->JointInternal.Type->size[0] = 1;
14915 obj->JointInternal.Type->size[1] = 8;
14916 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
14917 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14918 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
14919 }
14920
14921 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
14922 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14923 switch_expression->size[0] = 1;
14924 switch_expression->size[1] = obj->JointInternal.Type->size[1];
14925 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
14926 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
14927 - 1;
14928 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14929 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
14930 }
14931
14932 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14933 b[b_kstr] = tmp_5[b_kstr];
14934 }
14935
14936 b_bool = false;
14937 if (switch_expression->size[1] == 8) {
14938 b_kstr = 1;
14939 do {
14940 exitg1 = 0;
14941 if (b_kstr - 1 < 8) {
14942 loop_ub = b_kstr - 1;
14943 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14944 exitg1 = 1;
14945 } else {
14946 b_kstr++;
14947 }
14948 } else {
14949 b_bool = true;
14950 exitg1 = 1;
14951 }
14952 } while (exitg1 == 0);
14953 }
14954
14955 if (b_bool) {
14956 b_kstr = 0;
14957 } else {
14958 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14959 b_0[b_kstr] = tmp_6[b_kstr];
14960 }
14961
14962 b_bool = false;
14963 if (switch_expression->size[1] == 9) {
14964 b_kstr = 1;
14965 do {
14966 exitg1 = 0;
14967 if (b_kstr - 1 < 9) {
14968 loop_ub = b_kstr - 1;
14969 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14970 exitg1 = 1;
14971 } else {
14972 b_kstr++;
14973 }
14974 } else {
14975 b_bool = true;
14976 exitg1 = 1;
14977 }
14978 } while (exitg1 == 0);
14979 }
14980
14981 if (b_bool) {
14982 b_kstr = 1;
14983 } else {
14984 b_kstr = -1;
14985 }
14986 }
14987
14988 cartesian_traj_emxFree_char_T_a(&switch_expression);
14989 switch (b_kstr) {
14990 case 0:
14991 tmp[0] = 0;
14992 tmp[1] = 0;
14993 tmp[2] = 1;
14994 tmp[3] = 0;
14995 tmp[4] = 0;
14996 tmp[5] = 0;
14997 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14998 msubspace_data[b_kstr] = tmp[b_kstr];
14999 }
15000
15001 poslim_data[0] = -3.1415926535897931;
15002 poslim_data[1] = 3.1415926535897931;
15003 obj->JointInternal.VelocityNumber = 1.0;
15004 obj->JointInternal.PositionNumber = 1.0;
15005 obj->JointInternal.JointAxisInternal[0] = 0.0;
15006 obj->JointInternal.JointAxisInternal[1] = 0.0;
15007 obj->JointInternal.JointAxisInternal[2] = 1.0;
15008 break;
15009
15010 case 1:
15011 tmp[0] = 0;
15012 tmp[1] = 0;
15013 tmp[2] = 0;
15014 tmp[3] = 0;
15015 tmp[4] = 0;
15016 tmp[5] = 1;
15017 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15018 msubspace_data[b_kstr] = tmp[b_kstr];
15019 }
15020
15021 poslim_data[0] = -0.5;
15022 poslim_data[1] = 0.5;
15023 obj->JointInternal.VelocityNumber = 1.0;
15024 obj->JointInternal.PositionNumber = 1.0;
15025 obj->JointInternal.JointAxisInternal[0] = 0.0;
15026 obj->JointInternal.JointAxisInternal[1] = 0.0;
15027 obj->JointInternal.JointAxisInternal[2] = 1.0;
15028 break;
15029
15030 default:
15031 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15032 msubspace_data[b_kstr] = 0;
15033 }
15034
15035 poslim_data[0] = 0.0;
15036 poslim_data[1] = 0.0;
15037 obj->JointInternal.VelocityNumber = 0.0;
15038 obj->JointInternal.PositionNumber = 0.0;
15039 obj->JointInternal.JointAxisInternal[0] = 0.0;
15040 obj->JointInternal.JointAxisInternal[1] = 0.0;
15041 obj->JointInternal.JointAxisInternal[2] = 0.0;
15042 break;
15043 }
15044
15045 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15046 obj->JointInternal.MotionSubspace->size[1];
15047 obj->JointInternal.MotionSubspace->size[0] = 6;
15048 obj->JointInternal.MotionSubspace->size[1] = 1;
15049 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15050 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15051 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15052 }
15053
15054 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15055 obj->JointInternal.PositionLimitsInternal->size[1];
15056 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15057 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15058 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15059 b_kstr);
15060 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15061 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15062 }
15063
15064 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15065 obj->JointInternal.HomePositionInternal->size[0] = 1;
15066 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15067 b_kstr);
15068 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15069 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15070 }
15071
15072 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15073 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
15074 }
15075
15076 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15077 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
15078 }
15079
15080 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15081 obj->JointInternal.MotionSubspace->size[1];
15082 obj->JointInternal.MotionSubspace->size[0] = 6;
15083 obj->JointInternal.MotionSubspace->size[1] = 1;
15084 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15085 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15086 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
15087 }
15088
15089 obj->JointInternal.InTree = true;
15090 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15091 obj->JointInternal.PositionLimitsInternal->size[1];
15092 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15093 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15094 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15095 b_kstr);
15096 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
15097 obj->JointInternal.PositionLimitsInternal->data
15098 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
15099 obj->JointInternal.JointAxisInternal[0] = 0.0;
15100 obj->JointInternal.JointAxisInternal[1] = 0.0;
15101 obj->JointInternal.JointAxisInternal[2] = -1.0;
15102 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15103 obj->JointInternal.HomePositionInternal->size[0] = 1;
15104 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15105 b_kstr);
15106 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15107 return b_obj;
15108}
15109
15110static v_robotics_manip_internal_Rig_T *RigidBody_RigidBo_astwhqf2azttx
15111 (v_robotics_manip_internal_Rig_T *obj)
15112{
15113 v_robotics_manip_internal_Rig_T *b_obj;
15114 int8_T msubspace_data[36];
15115 real_T poslim_data[12];
15116 emxArray_char_T_cartesian_tra_T *switch_expression;
15117 boolean_T b_bool;
15118 int32_T b_kstr;
15119 char_T b[8];
15120 char_T b_0[9];
15121 int32_T loop_ub;
15122 int8_T tmp[6];
15123 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
15124 '3' };
15125
15126 static const real_T tmp_1[9] = { 0.0123976159829631, 0.00022039108420015264,
15127 2.1332825710116445E-6, 0.00022039108420015264, 0.00014803877895089843,
15128 -9.2077339045129963E-5, 2.1332825710116445E-6, -9.2077339045129963E-5,
15129 0.012477575138803739 };
15130
15131 static const real_T tmp_2[36] = { 0.0123976159829631, 0.00022039108420015264,
15132 2.1332825710116445E-6, 0.0, -2.56217202436532E-5, 0.0010295844637787021,
15133 0.00022039108420015264, 0.00014803877895089843, -9.2077339045129963E-5,
15134 2.56217202436532E-5, 0.0, 0.0024737535112545539, 2.1332825710116445E-6,
15135 -9.2077339045129963E-5, 0.012477575138803739, -0.0010295844637787021,
15136 -0.0024737535112545539, 0.0, 0.0, 2.56217202436532E-5,
15137 -0.0010295844637787021, 0.0785942338762368, 0.0, 0.0, -2.56217202436532E-5,
15138 0.0, -0.0024737535112545539, 0.0, 0.0785942338762368, 0.0,
15139 0.0010295844637787021, 0.0024737535112545539, 0.0, 0.0, 0.0,
15140 0.0785942338762368 };
15141
15142 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15143 1 };
15144
15145 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
15146 '_', '3' };
15147
15148 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15149
15150 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15151
15152 static const real_T tmp_7[16] = { 1.0, 2.0682310711021444E-13,
15153 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
15154 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
15155 1.0 };
15156
15157 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
15158 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
15159
15160 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
15161 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
15162 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
15163
15164 int32_T exitg1;
15165 b_obj = obj;
15166 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15167 obj->NameInternal->size[0] = 1;
15168 obj->NameInternal->size[1] = 10;
15169 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
15170 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15171 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15172 }
15173
15174 obj->ParentIndex = 3.0;
15175 obj->MassInternal = 0.0785942338762368;
15176 obj->CenterOfMassInternal[0] = -0.031475;
15177 obj->CenterOfMassInternal[1] = 0.0131;
15178 obj->CenterOfMassInternal[2] = 0.000326;
15179 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15180 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
15181 }
15182
15183 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
15184 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
15185 }
15186
15187 obj->JointInternal.InTree = false;
15188 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15189 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
15190 }
15191
15192 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15193 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
15194 }
15195
15196 b_kstr = obj->JointInternal.NameInternal->size[0] *
15197 obj->JointInternal.NameInternal->size[1];
15198 obj->JointInternal.NameInternal->size[0] = 1;
15199 obj->JointInternal.NameInternal->size[1] = 11;
15200 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.NameInternal, b_kstr);
15201 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
15202 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
15203 }
15204
15205 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
15206 obj->JointInternal.Type->size[0] = 1;
15207 obj->JointInternal.Type->size[1] = 8;
15208 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
15209 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15210 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
15211 }
15212
15213 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
15214 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15215 switch_expression->size[0] = 1;
15216 switch_expression->size[1] = obj->JointInternal.Type->size[1];
15217 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
15218 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
15219 - 1;
15220 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15221 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
15222 }
15223
15224 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15225 b[b_kstr] = tmp_5[b_kstr];
15226 }
15227
15228 b_bool = false;
15229 if (switch_expression->size[1] == 8) {
15230 b_kstr = 1;
15231 do {
15232 exitg1 = 0;
15233 if (b_kstr - 1 < 8) {
15234 loop_ub = b_kstr - 1;
15235 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15236 exitg1 = 1;
15237 } else {
15238 b_kstr++;
15239 }
15240 } else {
15241 b_bool = true;
15242 exitg1 = 1;
15243 }
15244 } while (exitg1 == 0);
15245 }
15246
15247 if (b_bool) {
15248 b_kstr = 0;
15249 } else {
15250 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15251 b_0[b_kstr] = tmp_6[b_kstr];
15252 }
15253
15254 b_bool = false;
15255 if (switch_expression->size[1] == 9) {
15256 b_kstr = 1;
15257 do {
15258 exitg1 = 0;
15259 if (b_kstr - 1 < 9) {
15260 loop_ub = b_kstr - 1;
15261 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15262 exitg1 = 1;
15263 } else {
15264 b_kstr++;
15265 }
15266 } else {
15267 b_bool = true;
15268 exitg1 = 1;
15269 }
15270 } while (exitg1 == 0);
15271 }
15272
15273 if (b_bool) {
15274 b_kstr = 1;
15275 } else {
15276 b_kstr = -1;
15277 }
15278 }
15279
15280 cartesian_traj_emxFree_char_T_a(&switch_expression);
15281 switch (b_kstr) {
15282 case 0:
15283 tmp[0] = 0;
15284 tmp[1] = 0;
15285 tmp[2] = 1;
15286 tmp[3] = 0;
15287 tmp[4] = 0;
15288 tmp[5] = 0;
15289 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15290 msubspace_data[b_kstr] = tmp[b_kstr];
15291 }
15292
15293 poslim_data[0] = -3.1415926535897931;
15294 poslim_data[1] = 3.1415926535897931;
15295 obj->JointInternal.VelocityNumber = 1.0;
15296 obj->JointInternal.PositionNumber = 1.0;
15297 obj->JointInternal.JointAxisInternal[0] = 0.0;
15298 obj->JointInternal.JointAxisInternal[1] = 0.0;
15299 obj->JointInternal.JointAxisInternal[2] = 1.0;
15300 break;
15301
15302 case 1:
15303 tmp[0] = 0;
15304 tmp[1] = 0;
15305 tmp[2] = 0;
15306 tmp[3] = 0;
15307 tmp[4] = 0;
15308 tmp[5] = 1;
15309 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15310 msubspace_data[b_kstr] = tmp[b_kstr];
15311 }
15312
15313 poslim_data[0] = -0.5;
15314 poslim_data[1] = 0.5;
15315 obj->JointInternal.VelocityNumber = 1.0;
15316 obj->JointInternal.PositionNumber = 1.0;
15317 obj->JointInternal.JointAxisInternal[0] = 0.0;
15318 obj->JointInternal.JointAxisInternal[1] = 0.0;
15319 obj->JointInternal.JointAxisInternal[2] = 1.0;
15320 break;
15321
15322 default:
15323 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15324 msubspace_data[b_kstr] = 0;
15325 }
15326
15327 poslim_data[0] = 0.0;
15328 poslim_data[1] = 0.0;
15329 obj->JointInternal.VelocityNumber = 0.0;
15330 obj->JointInternal.PositionNumber = 0.0;
15331 obj->JointInternal.JointAxisInternal[0] = 0.0;
15332 obj->JointInternal.JointAxisInternal[1] = 0.0;
15333 obj->JointInternal.JointAxisInternal[2] = 0.0;
15334 break;
15335 }
15336
15337 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15338 obj->JointInternal.MotionSubspace->size[1];
15339 obj->JointInternal.MotionSubspace->size[0] = 6;
15340 obj->JointInternal.MotionSubspace->size[1] = 1;
15341 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15342 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15343 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15344 }
15345
15346 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15347 obj->JointInternal.PositionLimitsInternal->size[1];
15348 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15349 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15350 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15351 b_kstr);
15352 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15353 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15354 }
15355
15356 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15357 obj->JointInternal.HomePositionInternal->size[0] = 1;
15358 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15359 b_kstr);
15360 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15361 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15362 }
15363
15364 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15365 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
15366 }
15367
15368 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15369 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
15370 }
15371
15372 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15373 obj->JointInternal.MotionSubspace->size[1];
15374 obj->JointInternal.MotionSubspace->size[0] = 6;
15375 obj->JointInternal.MotionSubspace->size[1] = 1;
15376 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15377 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15378 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
15379 }
15380
15381 obj->JointInternal.InTree = true;
15382 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15383 obj->JointInternal.PositionLimitsInternal->size[1];
15384 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15385 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15386 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15387 b_kstr);
15388 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
15389 obj->JointInternal.PositionLimitsInternal->data
15390 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
15391 obj->JointInternal.JointAxisInternal[0] = 0.0;
15392 obj->JointInternal.JointAxisInternal[1] = 0.0;
15393 obj->JointInternal.JointAxisInternal[2] = 1.0;
15394 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15395 obj->JointInternal.HomePositionInternal->size[0] = 1;
15396 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15397 b_kstr);
15398 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15399 return b_obj;
15400}
15401
15402static v_robotics_manip_internal_Rig_T *RigidBody_RigidB_astwhqf2azttxa
15403 (v_robotics_manip_internal_Rig_T *obj)
15404{
15405 v_robotics_manip_internal_Rig_T *b_obj;
15406 int8_T msubspace_data[36];
15407 real_T poslim_data[12];
15408 emxArray_char_T_cartesian_tra_T *switch_expression;
15409 boolean_T b_bool;
15410 int32_T b_kstr;
15411 char_T b[8];
15412 char_T b_0[9];
15413 int32_T loop_ub;
15414 int8_T tmp[6];
15415 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
15416 '4' };
15417
15418 static const real_T tmp_1[9] = { 0.0123990349928174, -2.7766271471639167E-6,
15419 0.00022466935228286869, -2.7766271471639167E-6, 0.012491487094789458,
15420 9.2330220708293281E-5, 0.00022466935228286869, 9.2330220708293281E-5,
15421 0.00016056153744711284 };
15422
15423 static const real_T tmp_2[36] = { 0.0123990349928174, -2.7766271471639167E-6,
15424 0.00022466935228286869, 0.0, 0.0010818496293063995, 4.275526322867282E-5,
15425 -2.7766271471639167E-6, 0.012491487094789458, 9.2330220708293281E-5,
15426 -0.0010818496293063995, 0.0, -0.0026650518765093138, 0.00022466935228286869,
15427 9.2330220708293281E-5, 0.00016056153744711284, -4.275526322867282E-5,
15428 0.0026650518765093138, 0.0, 0.0, -0.0010818496293063995,
15429 -4.275526322867282E-5, 0.0785942338762368, 0.0, 0.0, 0.0010818496293063995,
15430 0.0, 0.0026650518765093138, 0.0, 0.0785942338762368, 0.0,
15431 4.275526322867282E-5, -0.0026650518765093138, 0.0, 0.0, 0.0,
15432 0.0785942338762368 };
15433
15434 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15435 1 };
15436
15437 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
15438 '_', '4' };
15439
15440 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15441
15442 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15443
15444 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
15445 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
15446 0.0, -0.268, 0.0, 1.0 };
15447
15448 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
15449 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
15450
15451 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
15452 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
15453 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
15454
15455 int32_T exitg1;
15456 b_obj = obj;
15457 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15458 obj->NameInternal->size[0] = 1;
15459 obj->NameInternal->size[1] = 10;
15460 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
15461 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15462 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15463 }
15464
15465 obj->ParentIndex = 4.0;
15466 obj->MassInternal = 0.0785942338762368;
15467 obj->CenterOfMassInternal[0] = 0.033909;
15468 obj->CenterOfMassInternal[1] = 0.000544;
15469 obj->CenterOfMassInternal[2] = -0.013765;
15470 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15471 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
15472 }
15473
15474 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
15475 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
15476 }
15477
15478 obj->JointInternal.InTree = false;
15479 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15480 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
15481 }
15482
15483 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15484 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
15485 }
15486
15487 b_kstr = obj->JointInternal.NameInternal->size[0] *
15488 obj->JointInternal.NameInternal->size[1];
15489 obj->JointInternal.NameInternal->size[0] = 1;
15490 obj->JointInternal.NameInternal->size[1] = 11;
15491 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.NameInternal, b_kstr);
15492 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
15493 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
15494 }
15495
15496 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
15497 obj->JointInternal.Type->size[0] = 1;
15498 obj->JointInternal.Type->size[1] = 8;
15499 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
15500 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15501 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
15502 }
15503
15504 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
15505 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15506 switch_expression->size[0] = 1;
15507 switch_expression->size[1] = obj->JointInternal.Type->size[1];
15508 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
15509 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
15510 - 1;
15511 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15512 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
15513 }
15514
15515 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15516 b[b_kstr] = tmp_5[b_kstr];
15517 }
15518
15519 b_bool = false;
15520 if (switch_expression->size[1] == 8) {
15521 b_kstr = 1;
15522 do {
15523 exitg1 = 0;
15524 if (b_kstr - 1 < 8) {
15525 loop_ub = b_kstr - 1;
15526 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15527 exitg1 = 1;
15528 } else {
15529 b_kstr++;
15530 }
15531 } else {
15532 b_bool = true;
15533 exitg1 = 1;
15534 }
15535 } while (exitg1 == 0);
15536 }
15537
15538 if (b_bool) {
15539 b_kstr = 0;
15540 } else {
15541 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15542 b_0[b_kstr] = tmp_6[b_kstr];
15543 }
15544
15545 b_bool = false;
15546 if (switch_expression->size[1] == 9) {
15547 b_kstr = 1;
15548 do {
15549 exitg1 = 0;
15550 if (b_kstr - 1 < 9) {
15551 loop_ub = b_kstr - 1;
15552 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15553 exitg1 = 1;
15554 } else {
15555 b_kstr++;
15556 }
15557 } else {
15558 b_bool = true;
15559 exitg1 = 1;
15560 }
15561 } while (exitg1 == 0);
15562 }
15563
15564 if (b_bool) {
15565 b_kstr = 1;
15566 } else {
15567 b_kstr = -1;
15568 }
15569 }
15570
15571 cartesian_traj_emxFree_char_T_a(&switch_expression);
15572 switch (b_kstr) {
15573 case 0:
15574 tmp[0] = 0;
15575 tmp[1] = 0;
15576 tmp[2] = 1;
15577 tmp[3] = 0;
15578 tmp[4] = 0;
15579 tmp[5] = 0;
15580 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15581 msubspace_data[b_kstr] = tmp[b_kstr];
15582 }
15583
15584 poslim_data[0] = -3.1415926535897931;
15585 poslim_data[1] = 3.1415926535897931;
15586 obj->JointInternal.VelocityNumber = 1.0;
15587 obj->JointInternal.PositionNumber = 1.0;
15588 obj->JointInternal.JointAxisInternal[0] = 0.0;
15589 obj->JointInternal.JointAxisInternal[1] = 0.0;
15590 obj->JointInternal.JointAxisInternal[2] = 1.0;
15591 break;
15592
15593 case 1:
15594 tmp[0] = 0;
15595 tmp[1] = 0;
15596 tmp[2] = 0;
15597 tmp[3] = 0;
15598 tmp[4] = 0;
15599 tmp[5] = 1;
15600 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15601 msubspace_data[b_kstr] = tmp[b_kstr];
15602 }
15603
15604 poslim_data[0] = -0.5;
15605 poslim_data[1] = 0.5;
15606 obj->JointInternal.VelocityNumber = 1.0;
15607 obj->JointInternal.PositionNumber = 1.0;
15608 obj->JointInternal.JointAxisInternal[0] = 0.0;
15609 obj->JointInternal.JointAxisInternal[1] = 0.0;
15610 obj->JointInternal.JointAxisInternal[2] = 1.0;
15611 break;
15612
15613 default:
15614 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15615 msubspace_data[b_kstr] = 0;
15616 }
15617
15618 poslim_data[0] = 0.0;
15619 poslim_data[1] = 0.0;
15620 obj->JointInternal.VelocityNumber = 0.0;
15621 obj->JointInternal.PositionNumber = 0.0;
15622 obj->JointInternal.JointAxisInternal[0] = 0.0;
15623 obj->JointInternal.JointAxisInternal[1] = 0.0;
15624 obj->JointInternal.JointAxisInternal[2] = 0.0;
15625 break;
15626 }
15627
15628 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15629 obj->JointInternal.MotionSubspace->size[1];
15630 obj->JointInternal.MotionSubspace->size[0] = 6;
15631 obj->JointInternal.MotionSubspace->size[1] = 1;
15632 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15633 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15634 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15635 }
15636
15637 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15638 obj->JointInternal.PositionLimitsInternal->size[1];
15639 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15640 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15641 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15642 b_kstr);
15643 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15644 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15645 }
15646
15647 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15648 obj->JointInternal.HomePositionInternal->size[0] = 1;
15649 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15650 b_kstr);
15651 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15652 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15653 }
15654
15655 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15656 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
15657 }
15658
15659 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15660 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
15661 }
15662
15663 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15664 obj->JointInternal.MotionSubspace->size[1];
15665 obj->JointInternal.MotionSubspace->size[0] = 6;
15666 obj->JointInternal.MotionSubspace->size[1] = 1;
15667 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15668 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15669 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
15670 }
15671
15672 obj->JointInternal.InTree = true;
15673 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15674 obj->JointInternal.PositionLimitsInternal->size[1];
15675 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15676 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15677 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15678 b_kstr);
15679 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
15680 obj->JointInternal.PositionLimitsInternal->data
15681 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
15682 obj->JointInternal.JointAxisInternal[0] = 0.0;
15683 obj->JointInternal.JointAxisInternal[1] = 0.0;
15684 obj->JointInternal.JointAxisInternal[2] = 1.0;
15685 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15686 obj->JointInternal.HomePositionInternal->size[0] = 1;
15687 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15688 b_kstr);
15689 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15690 return b_obj;
15691}
15692
15693static v_robotics_manip_internal_Rig_T *RigidBody_Rigid_astwhqf2azttxab
15694 (v_robotics_manip_internal_Rig_T *obj)
15695{
15696 v_robotics_manip_internal_Rig_T *b_obj;
15697 int8_T msubspace_data[36];
15698 real_T poslim_data[12];
15699 emxArray_char_T_cartesian_tra_T *switch_expression;
15700 boolean_T b_bool;
15701 int32_T b_kstr;
15702 char_T b[8];
15703 char_T b_0[9];
15704 int32_T loop_ub;
15705 int8_T tmp[6];
15706 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
15707 '5' };
15708
15709 static const real_T tmp_1[9] = { 0.012440329403329006, 2.388185677857016E-6,
15710 0.00012602126519218373, 2.388185677857016E-6, 0.012510746127660349,
15711 -9.077919321137075E-5, 0.00012602126519218373, -9.077919321137075E-5,
15712 0.00013851261456175015 };
15713
15714 static const real_T tmp_2[36] = { 0.012440329403329006, 2.388185677857016E-6,
15715 0.00012602126519218373, 0.0, -0.0021015312196166957, -3.5996159115316455E-5,
15716 2.388185677857016E-6, 0.012510746127660349, -9.077919321137075E-5,
15717 0.0021015312196166957, 0.0, -0.0023173509858408423, 0.00012602126519218373,
15718 -9.077919321137075E-5, 0.00013851261456175015, 3.5996159115316455E-5,
15719 0.0023173509858408423, 0.0, 0.0, 0.0021015312196166957,
15720 3.5996159115316455E-5, 0.0785942338762368, 0.0, 0.0, -0.0021015312196166957,
15721 0.0, 0.0023173509858408423, 0.0, 0.0785942338762368, 0.0,
15722 -3.5996159115316455E-5, -0.0023173509858408423, 0.0, 0.0, 0.0,
15723 0.0785942338762368 };
15724
15725 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15726 1 };
15727
15728 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
15729 '_', '5' };
15730
15731 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15732
15733 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15734
15735 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
15736 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
15737
15738 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
15739 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
15740
15741 static const real_T tmp_9[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
15742 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
15743 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
15744
15745 int32_T exitg1;
15746 b_obj = obj;
15747 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15748 obj->NameInternal->size[0] = 1;
15749 obj->NameInternal->size[1] = 10;
15750 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
15751 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15752 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15753 }
15754
15755 obj->ParentIndex = 5.0;
15756 obj->MassInternal = 0.0785942338762368;
15757 obj->CenterOfMassInternal[0] = 0.029485;
15758 obj->CenterOfMassInternal[1] = -0.000458;
15759 obj->CenterOfMassInternal[2] = 0.026739;
15760 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15761 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
15762 }
15763
15764 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
15765 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
15766 }
15767
15768 obj->JointInternal.InTree = false;
15769 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15770 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
15771 }
15772
15773 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15774 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
15775 }
15776
15777 b_kstr = obj->JointInternal.NameInternal->size[0] *
15778 obj->JointInternal.NameInternal->size[1];
15779 obj->JointInternal.NameInternal->size[0] = 1;
15780 obj->JointInternal.NameInternal->size[1] = 11;
15781 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.NameInternal, b_kstr);
15782 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
15783 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
15784 }
15785
15786 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
15787 obj->JointInternal.Type->size[0] = 1;
15788 obj->JointInternal.Type->size[1] = 8;
15789 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
15790 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15791 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
15792 }
15793
15794 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
15795 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15796 switch_expression->size[0] = 1;
15797 switch_expression->size[1] = obj->JointInternal.Type->size[1];
15798 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
15799 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
15800 - 1;
15801 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15802 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
15803 }
15804
15805 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15806 b[b_kstr] = tmp_5[b_kstr];
15807 }
15808
15809 b_bool = false;
15810 if (switch_expression->size[1] == 8) {
15811 b_kstr = 1;
15812 do {
15813 exitg1 = 0;
15814 if (b_kstr - 1 < 8) {
15815 loop_ub = b_kstr - 1;
15816 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15817 exitg1 = 1;
15818 } else {
15819 b_kstr++;
15820 }
15821 } else {
15822 b_bool = true;
15823 exitg1 = 1;
15824 }
15825 } while (exitg1 == 0);
15826 }
15827
15828 if (b_bool) {
15829 b_kstr = 0;
15830 } else {
15831 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15832 b_0[b_kstr] = tmp_6[b_kstr];
15833 }
15834
15835 b_bool = false;
15836 if (switch_expression->size[1] == 9) {
15837 b_kstr = 1;
15838 do {
15839 exitg1 = 0;
15840 if (b_kstr - 1 < 9) {
15841 loop_ub = b_kstr - 1;
15842 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15843 exitg1 = 1;
15844 } else {
15845 b_kstr++;
15846 }
15847 } else {
15848 b_bool = true;
15849 exitg1 = 1;
15850 }
15851 } while (exitg1 == 0);
15852 }
15853
15854 if (b_bool) {
15855 b_kstr = 1;
15856 } else {
15857 b_kstr = -1;
15858 }
15859 }
15860
15861 cartesian_traj_emxFree_char_T_a(&switch_expression);
15862 switch (b_kstr) {
15863 case 0:
15864 tmp[0] = 0;
15865 tmp[1] = 0;
15866 tmp[2] = 1;
15867 tmp[3] = 0;
15868 tmp[4] = 0;
15869 tmp[5] = 0;
15870 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15871 msubspace_data[b_kstr] = tmp[b_kstr];
15872 }
15873
15874 poslim_data[0] = -3.1415926535897931;
15875 poslim_data[1] = 3.1415926535897931;
15876 obj->JointInternal.VelocityNumber = 1.0;
15877 obj->JointInternal.PositionNumber = 1.0;
15878 obj->JointInternal.JointAxisInternal[0] = 0.0;
15879 obj->JointInternal.JointAxisInternal[1] = 0.0;
15880 obj->JointInternal.JointAxisInternal[2] = 1.0;
15881 break;
15882
15883 case 1:
15884 tmp[0] = 0;
15885 tmp[1] = 0;
15886 tmp[2] = 0;
15887 tmp[3] = 0;
15888 tmp[4] = 0;
15889 tmp[5] = 1;
15890 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15891 msubspace_data[b_kstr] = tmp[b_kstr];
15892 }
15893
15894 poslim_data[0] = -0.5;
15895 poslim_data[1] = 0.5;
15896 obj->JointInternal.VelocityNumber = 1.0;
15897 obj->JointInternal.PositionNumber = 1.0;
15898 obj->JointInternal.JointAxisInternal[0] = 0.0;
15899 obj->JointInternal.JointAxisInternal[1] = 0.0;
15900 obj->JointInternal.JointAxisInternal[2] = 1.0;
15901 break;
15902
15903 default:
15904 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15905 msubspace_data[b_kstr] = 0;
15906 }
15907
15908 poslim_data[0] = 0.0;
15909 poslim_data[1] = 0.0;
15910 obj->JointInternal.VelocityNumber = 0.0;
15911 obj->JointInternal.PositionNumber = 0.0;
15912 obj->JointInternal.JointAxisInternal[0] = 0.0;
15913 obj->JointInternal.JointAxisInternal[1] = 0.0;
15914 obj->JointInternal.JointAxisInternal[2] = 0.0;
15915 break;
15916 }
15917
15918 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15919 obj->JointInternal.MotionSubspace->size[1];
15920 obj->JointInternal.MotionSubspace->size[0] = 6;
15921 obj->JointInternal.MotionSubspace->size[1] = 1;
15922 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15923 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15924 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15925 }
15926
15927 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15928 obj->JointInternal.PositionLimitsInternal->size[1];
15929 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15930 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15931 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15932 b_kstr);
15933 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15934 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15935 }
15936
15937 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15938 obj->JointInternal.HomePositionInternal->size[0] = 1;
15939 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15940 b_kstr);
15941 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15942 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15943 }
15944
15945 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15946 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
15947 }
15948
15949 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15950 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
15951 }
15952
15953 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15954 obj->JointInternal.MotionSubspace->size[1];
15955 obj->JointInternal.MotionSubspace->size[0] = 6;
15956 obj->JointInternal.MotionSubspace->size[1] = 1;
15957 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15958 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15959 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
15960 }
15961
15962 obj->JointInternal.InTree = true;
15963 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15964 obj->JointInternal.PositionLimitsInternal->size[1];
15965 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15966 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15967 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15968 b_kstr);
15969 obj->JointInternal.PositionLimitsInternal->data[0] = -1.79768912955;
15970 obj->JointInternal.PositionLimitsInternal->data
15971 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.79768912955;
15972 obj->JointInternal.JointAxisInternal[0] = 0.0;
15973 obj->JointInternal.JointAxisInternal[1] = 1.0;
15974 obj->JointInternal.JointAxisInternal[2] = 0.0;
15975 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15976 obj->JointInternal.HomePositionInternal->size[0] = 1;
15977 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15978 b_kstr);
15979 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15980 return b_obj;
15981}
15982
15983static v_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_b
15984 (v_robotics_manip_internal_Rig_T *obj)
15985{
15986 v_robotics_manip_internal_Rig_T *b_obj;
15987 int8_T msubspace_data[36];
15988 real_T poslim_data[12];
15989 emxArray_char_T_cartesian_tra_T *switch_expression;
15990 boolean_T b_bool;
15991 int32_T b_kstr;
15992 char_T b[8];
15993 char_T b_0[9];
15994 int32_T loop_ub;
15995 int8_T tmp[6];
15996 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
15997 '6' };
15998
15999 static const real_T tmp_1[9] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
16000 5.3285284099072352E-9, 2.0153545938371486E-9, 1.4574493611914028E-5,
16001 1.6742291194075022E-9, 5.3285284099072352E-9, 1.6742291194075022E-9,
16002 1.006193323459457E-5 };
16003
16004 static const real_T tmp_2[36] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
16005 5.3285284099072352E-9, 0.0, -1.6782149839359722E-7, -0.00026087851925284686,
16006 2.0153545938371486E-9, 1.4574493611914028E-5, 1.6742291194075022E-9,
16007 1.6782149839359722E-7, 0.0, -1.957917481258634E-7, 5.3285284099072352E-9,
16008 1.6742291194075022E-9, 1.006193323459457E-5, 0.00026087851925284686,
16009 1.957917481258634E-7, 0.0, 0.0, 1.6782149839359722E-7,
16010 0.00026087851925284686, 0.0279702497322662, 0.0, 0.0, -1.6782149839359722E-7,
16011 0.0, 1.957917481258634E-7, 0.0, 0.0279702497322662, 0.0,
16012 -0.00026087851925284686, -1.957917481258634E-7, 0.0, 0.0, 0.0,
16013 0.0279702497322662 };
16014
16015 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16016 1 };
16017
16018 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
16019 '_', '6' };
16020
16021 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16022
16023 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16024
16025 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
16026 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
16027
16028 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
16029 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
16030
16031 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16032 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16033 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
16034
16035 int32_T exitg1;
16036 b_obj = obj;
16037 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
16038 obj->NameInternal->size[0] = 1;
16039 obj->NameInternal->size[1] = 10;
16040 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
16041 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
16042 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16043 }
16044
16045 obj->ParentIndex = 6.0;
16046 obj->MassInternal = 0.0279702497322662;
16047 obj->CenterOfMassInternal[0] = 7.0E-6;
16048 obj->CenterOfMassInternal[1] = -0.009327;
16049 obj->CenterOfMassInternal[2] = 6.0E-6;
16050 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16051 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
16052 }
16053
16054 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
16055 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
16056 }
16057
16058 obj->JointInternal.InTree = false;
16059 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16060 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
16061 }
16062
16063 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16064 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
16065 }
16066
16067 b_kstr = obj->JointInternal.NameInternal->size[0] *
16068 obj->JointInternal.NameInternal->size[1];
16069 obj->JointInternal.NameInternal->size[0] = 1;
16070 obj->JointInternal.NameInternal->size[1] = 11;
16071 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.NameInternal, b_kstr);
16072 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
16073 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
16074 }
16075
16076 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
16077 obj->JointInternal.Type->size[0] = 1;
16078 obj->JointInternal.Type->size[1] = 8;
16079 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
16080 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16081 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
16082 }
16083
16084 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
16085 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16086 switch_expression->size[0] = 1;
16087 switch_expression->size[1] = obj->JointInternal.Type->size[1];
16088 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
16089 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
16090 - 1;
16091 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16092 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
16093 }
16094
16095 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16096 b[b_kstr] = tmp_5[b_kstr];
16097 }
16098
16099 b_bool = false;
16100 if (switch_expression->size[1] == 8) {
16101 b_kstr = 1;
16102 do {
16103 exitg1 = 0;
16104 if (b_kstr - 1 < 8) {
16105 loop_ub = b_kstr - 1;
16106 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16107 exitg1 = 1;
16108 } else {
16109 b_kstr++;
16110 }
16111 } else {
16112 b_bool = true;
16113 exitg1 = 1;
16114 }
16115 } while (exitg1 == 0);
16116 }
16117
16118 if (b_bool) {
16119 b_kstr = 0;
16120 } else {
16121 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16122 b_0[b_kstr] = tmp_6[b_kstr];
16123 }
16124
16125 b_bool = false;
16126 if (switch_expression->size[1] == 9) {
16127 b_kstr = 1;
16128 do {
16129 exitg1 = 0;
16130 if (b_kstr - 1 < 9) {
16131 loop_ub = b_kstr - 1;
16132 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16133 exitg1 = 1;
16134 } else {
16135 b_kstr++;
16136 }
16137 } else {
16138 b_bool = true;
16139 exitg1 = 1;
16140 }
16141 } while (exitg1 == 0);
16142 }
16143
16144 if (b_bool) {
16145 b_kstr = 1;
16146 } else {
16147 b_kstr = -1;
16148 }
16149 }
16150
16151 cartesian_traj_emxFree_char_T_a(&switch_expression);
16152 switch (b_kstr) {
16153 case 0:
16154 tmp[0] = 0;
16155 tmp[1] = 0;
16156 tmp[2] = 1;
16157 tmp[3] = 0;
16158 tmp[4] = 0;
16159 tmp[5] = 0;
16160 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16161 msubspace_data[b_kstr] = tmp[b_kstr];
16162 }
16163
16164 poslim_data[0] = -3.1415926535897931;
16165 poslim_data[1] = 3.1415926535897931;
16166 obj->JointInternal.VelocityNumber = 1.0;
16167 obj->JointInternal.PositionNumber = 1.0;
16168 obj->JointInternal.JointAxisInternal[0] = 0.0;
16169 obj->JointInternal.JointAxisInternal[1] = 0.0;
16170 obj->JointInternal.JointAxisInternal[2] = 1.0;
16171 break;
16172
16173 case 1:
16174 tmp[0] = 0;
16175 tmp[1] = 0;
16176 tmp[2] = 0;
16177 tmp[3] = 0;
16178 tmp[4] = 0;
16179 tmp[5] = 1;
16180 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16181 msubspace_data[b_kstr] = tmp[b_kstr];
16182 }
16183
16184 poslim_data[0] = -0.5;
16185 poslim_data[1] = 0.5;
16186 obj->JointInternal.VelocityNumber = 1.0;
16187 obj->JointInternal.PositionNumber = 1.0;
16188 obj->JointInternal.JointAxisInternal[0] = 0.0;
16189 obj->JointInternal.JointAxisInternal[1] = 0.0;
16190 obj->JointInternal.JointAxisInternal[2] = 1.0;
16191 break;
16192
16193 default:
16194 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16195 msubspace_data[b_kstr] = 0;
16196 }
16197
16198 poslim_data[0] = 0.0;
16199 poslim_data[1] = 0.0;
16200 obj->JointInternal.VelocityNumber = 0.0;
16201 obj->JointInternal.PositionNumber = 0.0;
16202 obj->JointInternal.JointAxisInternal[0] = 0.0;
16203 obj->JointInternal.JointAxisInternal[1] = 0.0;
16204 obj->JointInternal.JointAxisInternal[2] = 0.0;
16205 break;
16206 }
16207
16208 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
16209 obj->JointInternal.MotionSubspace->size[1];
16210 obj->JointInternal.MotionSubspace->size[0] = 6;
16211 obj->JointInternal.MotionSubspace->size[1] = 1;
16212 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
16213 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16214 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16215 }
16216
16217 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
16218 obj->JointInternal.PositionLimitsInternal->size[1];
16219 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
16220 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
16221 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
16222 b_kstr);
16223 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16224 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
16225 }
16226
16227 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
16228 obj->JointInternal.HomePositionInternal->size[0] = 1;
16229 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
16230 b_kstr);
16231 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16232 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
16233 }
16234
16235 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16236 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
16237 }
16238
16239 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16240 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
16241 }
16242
16243 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
16244 obj->JointInternal.MotionSubspace->size[1];
16245 obj->JointInternal.MotionSubspace->size[0] = 6;
16246 obj->JointInternal.MotionSubspace->size[1] = 1;
16247 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
16248 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16249 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
16250 }
16251
16252 obj->JointInternal.InTree = true;
16253 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
16254 obj->JointInternal.PositionLimitsInternal->size[1];
16255 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
16256 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
16257 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
16258 b_kstr);
16259 obj->JointInternal.PositionLimitsInternal->data[0] = -4.71238898038;
16260 obj->JointInternal.PositionLimitsInternal->data
16261 [obj->JointInternal.PositionLimitsInternal->size[0]] = 4.71238898038;
16262 obj->JointInternal.JointAxisInternal[0] = 0.0;
16263 obj->JointInternal.JointAxisInternal[1] = 0.0;
16264 obj->JointInternal.JointAxisInternal[2] = 1.0;
16265 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
16266 obj->JointInternal.HomePositionInternal->size[0] = 1;
16267 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
16268 b_kstr);
16269 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
16270 return b_obj;
16271}
16272
16273static y_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_as
16274 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
16275 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
16276 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
16277 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
16278 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
16279 v_robotics_manip_internal_Rig_T *iobj_7)
16280{
16281 y_robotics_manip_internal_Rig_T *b_obj;
16282 v_robotics_manip_internal_Rig_T *obj_0;
16283 int8_T msubspace_data[36];
16284 real_T poslim_data[12];
16285 int8_T b_I[9];
16286 emxArray_char_T_cartesian_tra_T *switch_expression;
16287 boolean_T b_bool;
16288 int32_T b_kstr;
16289 char_T b[8];
16290 char_T b_0[9];
16291 int32_T loop_ub;
16292 int8_T tmp[6];
16293 static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
16294 'e', 'e' };
16295
16296 static const real_T tmp_1[36] = { 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0,
16297 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0,
16298 -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0 };
16299
16300 static const int8_T tmp_2[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16301 1 };
16302
16303 static const char_T tmp_3[12] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
16304 '_', 'e', 'e' };
16305
16306 static const char_T tmp_4[5] = { 'f', 'i', 'x', 'e', 'd' };
16307
16308 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16309
16310 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16311
16312 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
16313 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
16314
16315 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
16316 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
16317
16318 static const char_T tmp_9[5] = { 'w', 'o', 'r', 'l', 'd' };
16319
16320 static const char_T tmp_a[9] = { 'w', 'o', 'r', 'l', 'd', '_', 'j', 'n', 't' };
16321
16322 int32_T exitg1;
16323 b_obj = obj;
16324 obj->Bodies[0] = RigidBody_RigidBody_astwhqf2az(iobj_7);
16325 obj->Bodies[0]->Index = 1.0;
16326 obj->Bodies[1] = RigidBody_RigidBody_astwhqf2azt(iobj_0);
16327 obj->Bodies[1]->Index = 2.0;
16328 obj->Bodies[2] = RigidBody_RigidBod_astwhqf2aztt(iobj_1);
16329 obj->Bodies[2]->Index = 3.0;
16330 obj->Bodies[3] = RigidBody_RigidBo_astwhqf2azttx(iobj_2);
16331 obj->Bodies[3]->Index = 4.0;
16332 obj->Bodies[4] = RigidBody_RigidB_astwhqf2azttxa(iobj_3);
16333 obj->Bodies[4]->Index = 5.0;
16334 obj->Bodies[5] = RigidBody_Rigid_astwhqf2azttxab(iobj_4);
16335 obj->Bodies[5]->Index = 6.0;
16336 obj->Bodies[6] = c_RigidBody_Rigid_b(iobj_5);
16337 obj->Bodies[6]->Index = 7.0;
16338 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
16339 iobj_6->NameInternal->size[0] = 1;
16340 iobj_6->NameInternal->size[1] = 11;
16341 cart_emxEnsureCapacity_char_T_a(iobj_6->NameInternal, b_kstr);
16342 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
16343 iobj_6->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16344 }
16345
16346 iobj_6->ParentIndex = 7.0;
16347 iobj_6->MassInternal = 0.0;
16348 iobj_6->CenterOfMassInternal[0] = 0.0;
16349 iobj_6->CenterOfMassInternal[1] = 0.0;
16350 iobj_6->CenterOfMassInternal[2] = 0.0;
16351 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16352 iobj_6->InertiaInternal[b_kstr] = 0.0;
16353 }
16354
16355 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
16356 iobj_6->SpatialInertia[b_kstr] = tmp_1[b_kstr];
16357 }
16358
16359 iobj_6->JointInternal.InTree = false;
16360 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16361 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
16362 }
16363
16364 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16365 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
16366 }
16367
16368 b_kstr = iobj_6->JointInternal.NameInternal->size[0] *
16369 iobj_6->JointInternal.NameInternal->size[1];
16370 iobj_6->JointInternal.NameInternal->size[0] = 1;
16371 iobj_6->JointInternal.NameInternal->size[1] = 12;
16372 cart_emxEnsureCapacity_char_T_a(iobj_6->JointInternal.NameInternal, b_kstr);
16373 for (b_kstr = 0; b_kstr < 12; b_kstr++) {
16374 iobj_6->JointInternal.NameInternal->data[b_kstr] = tmp_3[b_kstr];
16375 }
16376
16377 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
16378 JointInternal.Type->size[1];
16379 iobj_6->JointInternal.Type->size[0] = 1;
16380 iobj_6->JointInternal.Type->size[1] = 5;
16381 cart_emxEnsureCapacity_char_T_a(iobj_6->JointInternal.Type, b_kstr);
16382 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
16383 iobj_6->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
16384 }
16385
16386 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
16387 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16388 switch_expression->size[0] = 1;
16389 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
16390 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
16391 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
16392 JointInternal.Type->size[1] - 1;
16393 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16394 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
16395 }
16396
16397 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16398 b[b_kstr] = tmp_5[b_kstr];
16399 }
16400
16401 b_bool = false;
16402 if (switch_expression->size[1] == 8) {
16403 b_kstr = 1;
16404 do {
16405 exitg1 = 0;
16406 if (b_kstr - 1 < 8) {
16407 loop_ub = b_kstr - 1;
16408 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16409 exitg1 = 1;
16410 } else {
16411 b_kstr++;
16412 }
16413 } else {
16414 b_bool = true;
16415 exitg1 = 1;
16416 }
16417 } while (exitg1 == 0);
16418 }
16419
16420 if (b_bool) {
16421 b_kstr = 0;
16422 } else {
16423 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16424 b_0[b_kstr] = tmp_6[b_kstr];
16425 }
16426
16427 b_bool = false;
16428 if (switch_expression->size[1] == 9) {
16429 b_kstr = 1;
16430 do {
16431 exitg1 = 0;
16432 if (b_kstr - 1 < 9) {
16433 loop_ub = b_kstr - 1;
16434 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16435 exitg1 = 1;
16436 } else {
16437 b_kstr++;
16438 }
16439 } else {
16440 b_bool = true;
16441 exitg1 = 1;
16442 }
16443 } while (exitg1 == 0);
16444 }
16445
16446 if (b_bool) {
16447 b_kstr = 1;
16448 } else {
16449 b_kstr = -1;
16450 }
16451 }
16452
16453 switch (b_kstr) {
16454 case 0:
16455 tmp[0] = 0;
16456 tmp[1] = 0;
16457 tmp[2] = 1;
16458 tmp[3] = 0;
16459 tmp[4] = 0;
16460 tmp[5] = 0;
16461 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16462 msubspace_data[b_kstr] = tmp[b_kstr];
16463 }
16464
16465 poslim_data[0] = -3.1415926535897931;
16466 poslim_data[1] = 3.1415926535897931;
16467 iobj_6->JointInternal.VelocityNumber = 1.0;
16468 iobj_6->JointInternal.PositionNumber = 1.0;
16469 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
16470 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
16471 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
16472 break;
16473
16474 case 1:
16475 tmp[0] = 0;
16476 tmp[1] = 0;
16477 tmp[2] = 0;
16478 tmp[3] = 0;
16479 tmp[4] = 0;
16480 tmp[5] = 1;
16481 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16482 msubspace_data[b_kstr] = tmp[b_kstr];
16483 }
16484
16485 poslim_data[0] = -0.5;
16486 poslim_data[1] = 0.5;
16487 iobj_6->JointInternal.VelocityNumber = 1.0;
16488 iobj_6->JointInternal.PositionNumber = 1.0;
16489 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
16490 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
16491 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
16492 break;
16493
16494 default:
16495 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16496 msubspace_data[b_kstr] = 0;
16497 }
16498
16499 poslim_data[0] = 0.0;
16500 poslim_data[1] = 0.0;
16501 iobj_6->JointInternal.VelocityNumber = 0.0;
16502 iobj_6->JointInternal.PositionNumber = 0.0;
16503 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
16504 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
16505 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
16506 break;
16507 }
16508
16509 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
16510 iobj_6->JointInternal.MotionSubspace->size[1];
16511 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
16512 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
16513 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
16514 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16515 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16516 }
16517
16518 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
16519 iobj_6->JointInternal.PositionLimitsInternal->size[1];
16520 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
16521 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
16522 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
16523 b_kstr);
16524 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16525 iobj_6->JointInternal.PositionLimitsInternal->data[b_kstr] =
16526 poslim_data[b_kstr];
16527 }
16528
16529 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
16530 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
16531 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
16532 b_kstr);
16533 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16534 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
16535 }
16536
16537 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16538 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
16539 }
16540
16541 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16542 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
16543 }
16544
16545 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
16546 iobj_6->JointInternal.MotionSubspace->size[1];
16547 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
16548 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
16549 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
16550 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16551 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
16552 }
16553
16554 iobj_6->JointInternal.InTree = true;
16555 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
16556 iobj_6->JointInternal.PositionLimitsInternal->size[1];
16557 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
16558 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
16559 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
16560 b_kstr);
16561 iobj_6->JointInternal.PositionLimitsInternal->data[0] = 0.0;
16562 iobj_6->JointInternal.PositionLimitsInternal->data
16563 [iobj_6->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
16564 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
16565 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
16566 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
16567 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
16568 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
16569 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
16570 b_kstr);
16571 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
16572 obj->Bodies[7] = iobj_6;
16573 obj->Bodies[7]->Index = 8.0;
16574 obj->NumBodies = 8.0;
16575 obj->Gravity[0] = 0.0;
16576 obj->Gravity[1] = 0.0;
16577 obj->Gravity[2] = 0.0;
16578 obj_0 = &obj->Base;
16579 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
16580 obj->Base.NameInternal->size[0] = 1;
16581 obj->Base.NameInternal->size[1] = 5;
16582 cart_emxEnsureCapacity_char_T_a(obj->Base.NameInternal, b_kstr);
16583 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
16584 obj->Base.NameInternal->data[b_kstr] = tmp_9[b_kstr];
16585 }
16586
16587 obj->Base.JointInternal.InTree = false;
16588 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16589 obj_0->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
16590 }
16591
16592 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16593 obj_0->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
16594 }
16595
16596 b_kstr = obj->Base.JointInternal.NameInternal->size[0] *
16597 obj->Base.JointInternal.NameInternal->size[1];
16598 obj->Base.JointInternal.NameInternal->size[0] = 1;
16599 obj->Base.JointInternal.NameInternal->size[1] = 9;
16600 cart_emxEnsureCapacity_char_T_a(obj->Base.JointInternal.NameInternal, b_kstr);
16601 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16602 obj_0->JointInternal.NameInternal->data[b_kstr] = tmp_a[b_kstr];
16603 }
16604
16605 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
16606 Base.JointInternal.Type->size[1];
16607 obj->Base.JointInternal.Type->size[0] = 1;
16608 obj->Base.JointInternal.Type->size[1] = 5;
16609 cart_emxEnsureCapacity_char_T_a(obj->Base.JointInternal.Type, b_kstr);
16610 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
16611 obj_0->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
16612 }
16613
16614 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16615 switch_expression->size[0] = 1;
16616 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
16617 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
16618 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
16619 Base.JointInternal.Type->size[1] - 1;
16620 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16621 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
16622 }
16623
16624 b_bool = false;
16625 if (switch_expression->size[1] == 8) {
16626 b_kstr = 1;
16627 do {
16628 exitg1 = 0;
16629 if (b_kstr - 1 < 8) {
16630 loop_ub = b_kstr - 1;
16631 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16632 exitg1 = 1;
16633 } else {
16634 b_kstr++;
16635 }
16636 } else {
16637 b_bool = true;
16638 exitg1 = 1;
16639 }
16640 } while (exitg1 == 0);
16641 }
16642
16643 if (b_bool) {
16644 b_kstr = 0;
16645 } else {
16646 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16647 b_0[b_kstr] = tmp_6[b_kstr];
16648 }
16649
16650 b_bool = false;
16651 if (switch_expression->size[1] == 9) {
16652 b_kstr = 1;
16653 do {
16654 exitg1 = 0;
16655 if (b_kstr - 1 < 9) {
16656 loop_ub = b_kstr - 1;
16657 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16658 exitg1 = 1;
16659 } else {
16660 b_kstr++;
16661 }
16662 } else {
16663 b_bool = true;
16664 exitg1 = 1;
16665 }
16666 } while (exitg1 == 0);
16667 }
16668
16669 if (b_bool) {
16670 b_kstr = 1;
16671 } else {
16672 b_kstr = -1;
16673 }
16674 }
16675
16676 cartesian_traj_emxFree_char_T_a(&switch_expression);
16677 switch (b_kstr) {
16678 case 0:
16679 tmp[0] = 0;
16680 tmp[1] = 0;
16681 tmp[2] = 1;
16682 tmp[3] = 0;
16683 tmp[4] = 0;
16684 tmp[5] = 0;
16685 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16686 msubspace_data[b_kstr] = tmp[b_kstr];
16687 }
16688
16689 poslim_data[0] = -3.1415926535897931;
16690 poslim_data[1] = 3.1415926535897931;
16691 obj->Base.JointInternal.VelocityNumber = 1.0;
16692 obj->Base.JointInternal.PositionNumber = 1.0;
16693 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
16694 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
16695 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
16696 break;
16697
16698 case 1:
16699 tmp[0] = 0;
16700 tmp[1] = 0;
16701 tmp[2] = 0;
16702 tmp[3] = 0;
16703 tmp[4] = 0;
16704 tmp[5] = 1;
16705 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16706 msubspace_data[b_kstr] = tmp[b_kstr];
16707 }
16708
16709 poslim_data[0] = -0.5;
16710 poslim_data[1] = 0.5;
16711 obj->Base.JointInternal.VelocityNumber = 1.0;
16712 obj->Base.JointInternal.PositionNumber = 1.0;
16713 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
16714 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
16715 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
16716 break;
16717
16718 default:
16719 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16720 msubspace_data[b_kstr] = 0;
16721 }
16722
16723 poslim_data[0] = 0.0;
16724 poslim_data[1] = 0.0;
16725 obj->Base.JointInternal.VelocityNumber = 0.0;
16726 obj->Base.JointInternal.PositionNumber = 0.0;
16727 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
16728 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
16729 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
16730 break;
16731 }
16732
16733 b_kstr = obj->Base.JointInternal.MotionSubspace->size[0] *
16734 obj->Base.JointInternal.MotionSubspace->size[1];
16735 obj->Base.JointInternal.MotionSubspace->size[0] = 6;
16736 obj->Base.JointInternal.MotionSubspace->size[1] = 1;
16737 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.MotionSubspace, b_kstr);
16738 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16739 obj_0->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16740 }
16741
16742 b_kstr = obj->Base.JointInternal.PositionLimitsInternal->size[0] *
16743 obj->Base.JointInternal.PositionLimitsInternal->size[1];
16744 obj->Base.JointInternal.PositionLimitsInternal->size[0] = 1;
16745 obj->Base.JointInternal.PositionLimitsInternal->size[1] = 2;
16746 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.PositionLimitsInternal,
16747 b_kstr);
16748 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16749 obj_0->JointInternal.PositionLimitsInternal->data[b_kstr] =
16750 poslim_data[b_kstr];
16751 }
16752
16753 b_kstr = obj->Base.JointInternal.HomePositionInternal->size[0];
16754 obj->Base.JointInternal.HomePositionInternal->size[0] = 1;
16755 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.HomePositionInternal,
16756 b_kstr);
16757 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16758 obj_0->JointInternal.HomePositionInternal->data[0] = 0.0;
16759 }
16760
16761 obj->Base.Index = -1.0;
16762 obj->Base.ParentIndex = -1.0;
16763 obj->Base.MassInternal = 1.0;
16764 obj->Base.CenterOfMassInternal[0] = 0.0;
16765 obj->Base.CenterOfMassInternal[1] = 0.0;
16766 obj->Base.CenterOfMassInternal[2] = 0.0;
16767 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16768 b_I[b_kstr] = 0;
16769 }
16770
16771 b_I[0] = 1;
16772 b_I[4] = 1;
16773 b_I[8] = 1;
16774 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16775 obj->Base.InertiaInternal[b_kstr] = b_I[b_kstr];
16776 }
16777
16778 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
16779 msubspace_data[b_kstr] = 0;
16780 }
16781
16782 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16783 msubspace_data[b_kstr + 6 * b_kstr] = 1;
16784 }
16785
16786 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
16787 obj->Base.SpatialInertia[b_kstr] = msubspace_data[b_kstr];
16788 }
16789
16790 return b_obj;
16791}
16792
16793static void cartesian_trajectory_plann_rand(real_T r[5])
16794{
16795 for (cartesian_trajectory_planner_B.b_k_h = 0;
16796 cartesian_trajectory_planner_B.b_k_h < 5;
16797 cartesian_trajectory_planner_B.b_k_h++) {
16798 memcpy(&cartesian_trajectory_planner_B.uv[0],
16799 &cartesian_trajectory_planner_DW.state_m[0], 625U * sizeof(uint32_T));
16800 cartesian__eml_rand_mt19937ar_a(cartesian_trajectory_planner_B.uv,
16801 cartesian_trajectory_planner_DW.state_m,
16802 &r[cartesian_trajectory_planner_B.b_k_h]);
16803 }
16804}
16805
16806static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_i
16807 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
16808{
16809 w_robotics_manip_internal_Rig_T *b_obj;
16810 int8_T msubspace_data[36];
16811 real_T poslim_data[12];
16812 emxArray_char_T_cartesian_tra_T *switch_expression;
16813 boolean_T b_bool;
16814 int32_T b_kstr;
16815 char_T b[8];
16816 char_T b_0[9];
16817 int32_T loop_ub;
16818 int8_T tmp[6];
16819 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16820 '\x01' };
16821
16822 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16823 1 };
16824
16825 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16826 '\x01', '_', 'j', 'n', 't' };
16827
16828 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
16829
16830 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16831
16832 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16833
16834 int32_T exitg1;
16835 b_obj = obj;
16836 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
16837 obj->NameInternal->size[0] = 1;
16838 obj->NameInternal->size[1] = 10;
16839 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
16840 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
16841 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16842 }
16843
16844 iobj_0->InTree = false;
16845 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16846 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
16847 }
16848
16849 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16850 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
16851 }
16852
16853 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
16854 iobj_0->NameInternal->size[0] = 1;
16855 iobj_0->NameInternal->size[1] = 14;
16856 cart_emxEnsureCapacity_char_T_a(iobj_0->NameInternal, b_kstr);
16857 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
16858 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
16859 }
16860
16861 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
16862 iobj_0->Type->size[0] = 1;
16863 iobj_0->Type->size[1] = 5;
16864 cart_emxEnsureCapacity_char_T_a(iobj_0->Type, b_kstr);
16865 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
16866 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
16867 }
16868
16869 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
16870 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16871 switch_expression->size[0] = 1;
16872 switch_expression->size[1] = iobj_0->Type->size[1];
16873 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
16874 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
16875 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16876 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
16877 }
16878
16879 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16880 b[b_kstr] = tmp_4[b_kstr];
16881 }
16882
16883 b_bool = false;
16884 if (switch_expression->size[1] == 8) {
16885 b_kstr = 1;
16886 do {
16887 exitg1 = 0;
16888 if (b_kstr - 1 < 8) {
16889 loop_ub = b_kstr - 1;
16890 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16891 exitg1 = 1;
16892 } else {
16893 b_kstr++;
16894 }
16895 } else {
16896 b_bool = true;
16897 exitg1 = 1;
16898 }
16899 } while (exitg1 == 0);
16900 }
16901
16902 if (b_bool) {
16903 b_kstr = 0;
16904 } else {
16905 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16906 b_0[b_kstr] = tmp_5[b_kstr];
16907 }
16908
16909 b_bool = false;
16910 if (switch_expression->size[1] == 9) {
16911 b_kstr = 1;
16912 do {
16913 exitg1 = 0;
16914 if (b_kstr - 1 < 9) {
16915 loop_ub = b_kstr - 1;
16916 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16917 exitg1 = 1;
16918 } else {
16919 b_kstr++;
16920 }
16921 } else {
16922 b_bool = true;
16923 exitg1 = 1;
16924 }
16925 } while (exitg1 == 0);
16926 }
16927
16928 if (b_bool) {
16929 b_kstr = 1;
16930 } else {
16931 b_kstr = -1;
16932 }
16933 }
16934
16935 cartesian_traj_emxFree_char_T_a(&switch_expression);
16936 switch (b_kstr) {
16937 case 0:
16938 tmp[0] = 0;
16939 tmp[1] = 0;
16940 tmp[2] = 1;
16941 tmp[3] = 0;
16942 tmp[4] = 0;
16943 tmp[5] = 0;
16944 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16945 msubspace_data[b_kstr] = tmp[b_kstr];
16946 }
16947
16948 poslim_data[0] = -3.1415926535897931;
16949 poslim_data[1] = 3.1415926535897931;
16950 iobj_0->VelocityNumber = 1.0;
16951 iobj_0->PositionNumber = 1.0;
16952 iobj_0->JointAxisInternal[0] = 0.0;
16953 iobj_0->JointAxisInternal[1] = 0.0;
16954 iobj_0->JointAxisInternal[2] = 1.0;
16955 break;
16956
16957 case 1:
16958 tmp[0] = 0;
16959 tmp[1] = 0;
16960 tmp[2] = 0;
16961 tmp[3] = 0;
16962 tmp[4] = 0;
16963 tmp[5] = 1;
16964 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16965 msubspace_data[b_kstr] = tmp[b_kstr];
16966 }
16967
16968 poslim_data[0] = -0.5;
16969 poslim_data[1] = 0.5;
16970 iobj_0->VelocityNumber = 1.0;
16971 iobj_0->PositionNumber = 1.0;
16972 iobj_0->JointAxisInternal[0] = 0.0;
16973 iobj_0->JointAxisInternal[1] = 0.0;
16974 iobj_0->JointAxisInternal[2] = 1.0;
16975 break;
16976
16977 default:
16978 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16979 msubspace_data[b_kstr] = 0;
16980 }
16981
16982 poslim_data[0] = 0.0;
16983 poslim_data[1] = 0.0;
16984 iobj_0->VelocityNumber = 0.0;
16985 iobj_0->PositionNumber = 0.0;
16986 iobj_0->JointAxisInternal[0] = 0.0;
16987 iobj_0->JointAxisInternal[1] = 0.0;
16988 iobj_0->JointAxisInternal[2] = 0.0;
16989 break;
16990 }
16991
16992 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
16993 iobj_0->MotionSubspace->size[0] = 6;
16994 iobj_0->MotionSubspace->size[1] = 1;
16995 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
16996 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16997 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16998 }
16999
17000 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
17001 iobj_0->PositionLimitsInternal->size[1];
17002 iobj_0->PositionLimitsInternal->size[0] = 1;
17003 iobj_0->PositionLimitsInternal->size[1] = 2;
17004 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
17005 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
17006 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
17007 }
17008
17009 b_kstr = iobj_0->HomePositionInternal->size[0];
17010 iobj_0->HomePositionInternal->size[0] = 1;
17011 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
17012 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
17013 iobj_0->HomePositionInternal->data[0] = 0.0;
17014 }
17015
17016 obj->JointInternal = iobj_0;
17017 obj->Index = -1.0;
17018 obj->ParentIndex = -1.0;
17019 return b_obj;
17020}
17021
17022static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_l
17023 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
17024{
17025 w_robotics_manip_internal_Rig_T *b_obj;
17026 int8_T msubspace_data[36];
17027 real_T poslim_data[12];
17028 emxArray_char_T_cartesian_tra_T *switch_expression;
17029 boolean_T b_bool;
17030 int32_T b_kstr;
17031 char_T b[8];
17032 char_T b_0[9];
17033 int32_T loop_ub;
17034 int8_T tmp[6];
17035 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17036 '\x02' };
17037
17038 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
17039 1 };
17040
17041 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17042 '\x02', '_', 'j', 'n', 't' };
17043
17044 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
17045
17046 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17047
17048 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17049
17050 int32_T exitg1;
17051 b_obj = obj;
17052 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
17053 obj->NameInternal->size[0] = 1;
17054 obj->NameInternal->size[1] = 10;
17055 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
17056 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
17057 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
17058 }
17059
17060 iobj_0->InTree = false;
17061 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17062 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
17063 }
17064
17065 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17066 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
17067 }
17068
17069 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
17070 iobj_0->NameInternal->size[0] = 1;
17071 iobj_0->NameInternal->size[1] = 14;
17072 cart_emxEnsureCapacity_char_T_a(iobj_0->NameInternal, b_kstr);
17073 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
17074 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
17075 }
17076
17077 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
17078 iobj_0->Type->size[0] = 1;
17079 iobj_0->Type->size[1] = 5;
17080 cart_emxEnsureCapacity_char_T_a(iobj_0->Type, b_kstr);
17081 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
17082 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
17083 }
17084
17085 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
17086 b_kstr = switch_expression->size[0] * switch_expression->size[1];
17087 switch_expression->size[0] = 1;
17088 switch_expression->size[1] = iobj_0->Type->size[1];
17089 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
17090 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
17091 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
17092 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
17093 }
17094
17095 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17096 b[b_kstr] = tmp_4[b_kstr];
17097 }
17098
17099 b_bool = false;
17100 if (switch_expression->size[1] == 8) {
17101 b_kstr = 1;
17102 do {
17103 exitg1 = 0;
17104 if (b_kstr - 1 < 8) {
17105 loop_ub = b_kstr - 1;
17106 if (switch_expression->data[loop_ub] != b[loop_ub]) {
17107 exitg1 = 1;
17108 } else {
17109 b_kstr++;
17110 }
17111 } else {
17112 b_bool = true;
17113 exitg1 = 1;
17114 }
17115 } while (exitg1 == 0);
17116 }
17117
17118 if (b_bool) {
17119 b_kstr = 0;
17120 } else {
17121 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17122 b_0[b_kstr] = tmp_5[b_kstr];
17123 }
17124
17125 b_bool = false;
17126 if (switch_expression->size[1] == 9) {
17127 b_kstr = 1;
17128 do {
17129 exitg1 = 0;
17130 if (b_kstr - 1 < 9) {
17131 loop_ub = b_kstr - 1;
17132 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
17133 exitg1 = 1;
17134 } else {
17135 b_kstr++;
17136 }
17137 } else {
17138 b_bool = true;
17139 exitg1 = 1;
17140 }
17141 } while (exitg1 == 0);
17142 }
17143
17144 if (b_bool) {
17145 b_kstr = 1;
17146 } else {
17147 b_kstr = -1;
17148 }
17149 }
17150
17151 cartesian_traj_emxFree_char_T_a(&switch_expression);
17152 switch (b_kstr) {
17153 case 0:
17154 tmp[0] = 0;
17155 tmp[1] = 0;
17156 tmp[2] = 1;
17157 tmp[3] = 0;
17158 tmp[4] = 0;
17159 tmp[5] = 0;
17160 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17161 msubspace_data[b_kstr] = tmp[b_kstr];
17162 }
17163
17164 poslim_data[0] = -3.1415926535897931;
17165 poslim_data[1] = 3.1415926535897931;
17166 iobj_0->VelocityNumber = 1.0;
17167 iobj_0->PositionNumber = 1.0;
17168 iobj_0->JointAxisInternal[0] = 0.0;
17169 iobj_0->JointAxisInternal[1] = 0.0;
17170 iobj_0->JointAxisInternal[2] = 1.0;
17171 break;
17172
17173 case 1:
17174 tmp[0] = 0;
17175 tmp[1] = 0;
17176 tmp[2] = 0;
17177 tmp[3] = 0;
17178 tmp[4] = 0;
17179 tmp[5] = 1;
17180 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17181 msubspace_data[b_kstr] = tmp[b_kstr];
17182 }
17183
17184 poslim_data[0] = -0.5;
17185 poslim_data[1] = 0.5;
17186 iobj_0->VelocityNumber = 1.0;
17187 iobj_0->PositionNumber = 1.0;
17188 iobj_0->JointAxisInternal[0] = 0.0;
17189 iobj_0->JointAxisInternal[1] = 0.0;
17190 iobj_0->JointAxisInternal[2] = 1.0;
17191 break;
17192
17193 default:
17194 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17195 msubspace_data[b_kstr] = 0;
17196 }
17197
17198 poslim_data[0] = 0.0;
17199 poslim_data[1] = 0.0;
17200 iobj_0->VelocityNumber = 0.0;
17201 iobj_0->PositionNumber = 0.0;
17202 iobj_0->JointAxisInternal[0] = 0.0;
17203 iobj_0->JointAxisInternal[1] = 0.0;
17204 iobj_0->JointAxisInternal[2] = 0.0;
17205 break;
17206 }
17207
17208 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
17209 iobj_0->MotionSubspace->size[0] = 6;
17210 iobj_0->MotionSubspace->size[1] = 1;
17211 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
17212 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17213 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
17214 }
17215
17216 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
17217 iobj_0->PositionLimitsInternal->size[1];
17218 iobj_0->PositionLimitsInternal->size[0] = 1;
17219 iobj_0->PositionLimitsInternal->size[1] = 2;
17220 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
17221 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
17222 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
17223 }
17224
17225 b_kstr = iobj_0->HomePositionInternal->size[0];
17226 iobj_0->HomePositionInternal->size[0] = 1;
17227 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
17228 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
17229 iobj_0->HomePositionInternal->data[0] = 0.0;
17230 }
17231
17232 obj->JointInternal = iobj_0;
17233 obj->Index = -1.0;
17234 obj->ParentIndex = -1.0;
17235 return b_obj;
17236}
17237
17238static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_lh
17239 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
17240{
17241 w_robotics_manip_internal_Rig_T *b_obj;
17242 int8_T msubspace_data[36];
17243 real_T poslim_data[12];
17244 emxArray_char_T_cartesian_tra_T *switch_expression;
17245 boolean_T b_bool;
17246 int32_T b_kstr;
17247 char_T b[8];
17248 char_T b_0[9];
17249 int32_T loop_ub;
17250 int8_T tmp[6];
17251 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17252 '\x03' };
17253
17254 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
17255 1 };
17256
17257 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17258 '\x03', '_', 'j', 'n', 't' };
17259
17260 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
17261
17262 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17263
17264 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17265
17266 int32_T exitg1;
17267 b_obj = obj;
17268 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
17269 obj->NameInternal->size[0] = 1;
17270 obj->NameInternal->size[1] = 10;
17271 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
17272 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
17273 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
17274 }
17275
17276 iobj_0->InTree = false;
17277 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17278 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
17279 }
17280
17281 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17282 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
17283 }
17284
17285 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
17286 iobj_0->NameInternal->size[0] = 1;
17287 iobj_0->NameInternal->size[1] = 14;
17288 cart_emxEnsureCapacity_char_T_a(iobj_0->NameInternal, b_kstr);
17289 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
17290 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
17291 }
17292
17293 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
17294 iobj_0->Type->size[0] = 1;
17295 iobj_0->Type->size[1] = 5;
17296 cart_emxEnsureCapacity_char_T_a(iobj_0->Type, b_kstr);
17297 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
17298 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
17299 }
17300
17301 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
17302 b_kstr = switch_expression->size[0] * switch_expression->size[1];
17303 switch_expression->size[0] = 1;
17304 switch_expression->size[1] = iobj_0->Type->size[1];
17305 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
17306 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
17307 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
17308 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
17309 }
17310
17311 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17312 b[b_kstr] = tmp_4[b_kstr];
17313 }
17314
17315 b_bool = false;
17316 if (switch_expression->size[1] == 8) {
17317 b_kstr = 1;
17318 do {
17319 exitg1 = 0;
17320 if (b_kstr - 1 < 8) {
17321 loop_ub = b_kstr - 1;
17322 if (switch_expression->data[loop_ub] != b[loop_ub]) {
17323 exitg1 = 1;
17324 } else {
17325 b_kstr++;
17326 }
17327 } else {
17328 b_bool = true;
17329 exitg1 = 1;
17330 }
17331 } while (exitg1 == 0);
17332 }
17333
17334 if (b_bool) {
17335 b_kstr = 0;
17336 } else {
17337 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17338 b_0[b_kstr] = tmp_5[b_kstr];
17339 }
17340
17341 b_bool = false;
17342 if (switch_expression->size[1] == 9) {
17343 b_kstr = 1;
17344 do {
17345 exitg1 = 0;
17346 if (b_kstr - 1 < 9) {
17347 loop_ub = b_kstr - 1;
17348 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
17349 exitg1 = 1;
17350 } else {
17351 b_kstr++;
17352 }
17353 } else {
17354 b_bool = true;
17355 exitg1 = 1;
17356 }
17357 } while (exitg1 == 0);
17358 }
17359
17360 if (b_bool) {
17361 b_kstr = 1;
17362 } else {
17363 b_kstr = -1;
17364 }
17365 }
17366
17367 cartesian_traj_emxFree_char_T_a(&switch_expression);
17368 switch (b_kstr) {
17369 case 0:
17370 tmp[0] = 0;
17371 tmp[1] = 0;
17372 tmp[2] = 1;
17373 tmp[3] = 0;
17374 tmp[4] = 0;
17375 tmp[5] = 0;
17376 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17377 msubspace_data[b_kstr] = tmp[b_kstr];
17378 }
17379
17380 poslim_data[0] = -3.1415926535897931;
17381 poslim_data[1] = 3.1415926535897931;
17382 iobj_0->VelocityNumber = 1.0;
17383 iobj_0->PositionNumber = 1.0;
17384 iobj_0->JointAxisInternal[0] = 0.0;
17385 iobj_0->JointAxisInternal[1] = 0.0;
17386 iobj_0->JointAxisInternal[2] = 1.0;
17387 break;
17388
17389 case 1:
17390 tmp[0] = 0;
17391 tmp[1] = 0;
17392 tmp[2] = 0;
17393 tmp[3] = 0;
17394 tmp[4] = 0;
17395 tmp[5] = 1;
17396 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17397 msubspace_data[b_kstr] = tmp[b_kstr];
17398 }
17399
17400 poslim_data[0] = -0.5;
17401 poslim_data[1] = 0.5;
17402 iobj_0->VelocityNumber = 1.0;
17403 iobj_0->PositionNumber = 1.0;
17404 iobj_0->JointAxisInternal[0] = 0.0;
17405 iobj_0->JointAxisInternal[1] = 0.0;
17406 iobj_0->JointAxisInternal[2] = 1.0;
17407 break;
17408
17409 default:
17410 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17411 msubspace_data[b_kstr] = 0;
17412 }
17413
17414 poslim_data[0] = 0.0;
17415 poslim_data[1] = 0.0;
17416 iobj_0->VelocityNumber = 0.0;
17417 iobj_0->PositionNumber = 0.0;
17418 iobj_0->JointAxisInternal[0] = 0.0;
17419 iobj_0->JointAxisInternal[1] = 0.0;
17420 iobj_0->JointAxisInternal[2] = 0.0;
17421 break;
17422 }
17423
17424 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
17425 iobj_0->MotionSubspace->size[0] = 6;
17426 iobj_0->MotionSubspace->size[1] = 1;
17427 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
17428 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17429 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
17430 }
17431
17432 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
17433 iobj_0->PositionLimitsInternal->size[1];
17434 iobj_0->PositionLimitsInternal->size[0] = 1;
17435 iobj_0->PositionLimitsInternal->size[1] = 2;
17436 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
17437 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
17438 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
17439 }
17440
17441 b_kstr = iobj_0->HomePositionInternal->size[0];
17442 iobj_0->HomePositionInternal->size[0] = 1;
17443 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
17444 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
17445 iobj_0->HomePositionInternal->data[0] = 0.0;
17446 }
17447
17448 obj->JointInternal = iobj_0;
17449 obj->Index = -1.0;
17450 obj->ParentIndex = -1.0;
17451 return b_obj;
17452}
17453
17454static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_k
17455 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
17456{
17457 w_robotics_manip_internal_Rig_T *b_obj;
17458 int8_T msubspace_data[36];
17459 real_T poslim_data[12];
17460 emxArray_char_T_cartesian_tra_T *switch_expression;
17461 boolean_T b_bool;
17462 int32_T b_kstr;
17463 char_T b[8];
17464 char_T b_0[9];
17465 int32_T loop_ub;
17466 int8_T tmp[6];
17467 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17468 '\x04' };
17469
17470 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
17471 1 };
17472
17473 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17474 '\x04', '_', 'j', 'n', 't' };
17475
17476 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
17477
17478 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17479
17480 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17481
17482 int32_T exitg1;
17483 b_obj = obj;
17484 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
17485 obj->NameInternal->size[0] = 1;
17486 obj->NameInternal->size[1] = 10;
17487 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
17488 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
17489 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
17490 }
17491
17492 iobj_0->InTree = false;
17493 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17494 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
17495 }
17496
17497 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17498 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
17499 }
17500
17501 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
17502 iobj_0->NameInternal->size[0] = 1;
17503 iobj_0->NameInternal->size[1] = 14;
17504 cart_emxEnsureCapacity_char_T_a(iobj_0->NameInternal, b_kstr);
17505 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
17506 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
17507 }
17508
17509 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
17510 iobj_0->Type->size[0] = 1;
17511 iobj_0->Type->size[1] = 5;
17512 cart_emxEnsureCapacity_char_T_a(iobj_0->Type, b_kstr);
17513 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
17514 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
17515 }
17516
17517 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
17518 b_kstr = switch_expression->size[0] * switch_expression->size[1];
17519 switch_expression->size[0] = 1;
17520 switch_expression->size[1] = iobj_0->Type->size[1];
17521 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
17522 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
17523 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
17524 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
17525 }
17526
17527 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17528 b[b_kstr] = tmp_4[b_kstr];
17529 }
17530
17531 b_bool = false;
17532 if (switch_expression->size[1] == 8) {
17533 b_kstr = 1;
17534 do {
17535 exitg1 = 0;
17536 if (b_kstr - 1 < 8) {
17537 loop_ub = b_kstr - 1;
17538 if (switch_expression->data[loop_ub] != b[loop_ub]) {
17539 exitg1 = 1;
17540 } else {
17541 b_kstr++;
17542 }
17543 } else {
17544 b_bool = true;
17545 exitg1 = 1;
17546 }
17547 } while (exitg1 == 0);
17548 }
17549
17550 if (b_bool) {
17551 b_kstr = 0;
17552 } else {
17553 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17554 b_0[b_kstr] = tmp_5[b_kstr];
17555 }
17556
17557 b_bool = false;
17558 if (switch_expression->size[1] == 9) {
17559 b_kstr = 1;
17560 do {
17561 exitg1 = 0;
17562 if (b_kstr - 1 < 9) {
17563 loop_ub = b_kstr - 1;
17564 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
17565 exitg1 = 1;
17566 } else {
17567 b_kstr++;
17568 }
17569 } else {
17570 b_bool = true;
17571 exitg1 = 1;
17572 }
17573 } while (exitg1 == 0);
17574 }
17575
17576 if (b_bool) {
17577 b_kstr = 1;
17578 } else {
17579 b_kstr = -1;
17580 }
17581 }
17582
17583 cartesian_traj_emxFree_char_T_a(&switch_expression);
17584 switch (b_kstr) {
17585 case 0:
17586 tmp[0] = 0;
17587 tmp[1] = 0;
17588 tmp[2] = 1;
17589 tmp[3] = 0;
17590 tmp[4] = 0;
17591 tmp[5] = 0;
17592 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17593 msubspace_data[b_kstr] = tmp[b_kstr];
17594 }
17595
17596 poslim_data[0] = -3.1415926535897931;
17597 poslim_data[1] = 3.1415926535897931;
17598 iobj_0->VelocityNumber = 1.0;
17599 iobj_0->PositionNumber = 1.0;
17600 iobj_0->JointAxisInternal[0] = 0.0;
17601 iobj_0->JointAxisInternal[1] = 0.0;
17602 iobj_0->JointAxisInternal[2] = 1.0;
17603 break;
17604
17605 case 1:
17606 tmp[0] = 0;
17607 tmp[1] = 0;
17608 tmp[2] = 0;
17609 tmp[3] = 0;
17610 tmp[4] = 0;
17611 tmp[5] = 1;
17612 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17613 msubspace_data[b_kstr] = tmp[b_kstr];
17614 }
17615
17616 poslim_data[0] = -0.5;
17617 poslim_data[1] = 0.5;
17618 iobj_0->VelocityNumber = 1.0;
17619 iobj_0->PositionNumber = 1.0;
17620 iobj_0->JointAxisInternal[0] = 0.0;
17621 iobj_0->JointAxisInternal[1] = 0.0;
17622 iobj_0->JointAxisInternal[2] = 1.0;
17623 break;
17624
17625 default:
17626 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17627 msubspace_data[b_kstr] = 0;
17628 }
17629
17630 poslim_data[0] = 0.0;
17631 poslim_data[1] = 0.0;
17632 iobj_0->VelocityNumber = 0.0;
17633 iobj_0->PositionNumber = 0.0;
17634 iobj_0->JointAxisInternal[0] = 0.0;
17635 iobj_0->JointAxisInternal[1] = 0.0;
17636 iobj_0->JointAxisInternal[2] = 0.0;
17637 break;
17638 }
17639
17640 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
17641 iobj_0->MotionSubspace->size[0] = 6;
17642 iobj_0->MotionSubspace->size[1] = 1;
17643 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
17644 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17645 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
17646 }
17647
17648 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
17649 iobj_0->PositionLimitsInternal->size[1];
17650 iobj_0->PositionLimitsInternal->size[0] = 1;
17651 iobj_0->PositionLimitsInternal->size[1] = 2;
17652 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
17653 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
17654 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
17655 }
17656
17657 b_kstr = iobj_0->HomePositionInternal->size[0];
17658 iobj_0->HomePositionInternal->size[0] = 1;
17659 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
17660 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
17661 iobj_0->HomePositionInternal->data[0] = 0.0;
17662 }
17663
17664 obj->JointInternal = iobj_0;
17665 obj->Index = -1.0;
17666 obj->ParentIndex = -1.0;
17667 return b_obj;
17668}
17669
17670static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_h
17671 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0)
17672{
17673 w_robotics_manip_internal_Rig_T *b_obj;
17674 int8_T msubspace_data[36];
17675 real_T poslim_data[12];
17676 emxArray_char_T_cartesian_tra_T *switch_expression;
17677 boolean_T b_bool;
17678 int32_T b_kstr;
17679 char_T b[8];
17680 char_T b_0[9];
17681 int32_T loop_ub;
17682 int8_T tmp[6];
17683 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17684 '\x05' };
17685
17686 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
17687 1 };
17688
17689 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17690 '\x05', '_', 'j', 'n', 't' };
17691
17692 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
17693
17694 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17695
17696 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17697
17698 int32_T exitg1;
17699 b_obj = obj;
17700 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
17701 obj->NameInternal->size[0] = 1;
17702 obj->NameInternal->size[1] = 10;
17703 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
17704 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
17705 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
17706 }
17707
17708 iobj_0->InTree = false;
17709 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17710 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
17711 }
17712
17713 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17714 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
17715 }
17716
17717 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
17718 iobj_0->NameInternal->size[0] = 1;
17719 iobj_0->NameInternal->size[1] = 14;
17720 cart_emxEnsureCapacity_char_T_a(iobj_0->NameInternal, b_kstr);
17721 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
17722 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
17723 }
17724
17725 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
17726 iobj_0->Type->size[0] = 1;
17727 iobj_0->Type->size[1] = 5;
17728 cart_emxEnsureCapacity_char_T_a(iobj_0->Type, b_kstr);
17729 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
17730 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
17731 }
17732
17733 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
17734 b_kstr = switch_expression->size[0] * switch_expression->size[1];
17735 switch_expression->size[0] = 1;
17736 switch_expression->size[1] = iobj_0->Type->size[1];
17737 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
17738 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
17739 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
17740 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
17741 }
17742
17743 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17744 b[b_kstr] = tmp_4[b_kstr];
17745 }
17746
17747 b_bool = false;
17748 if (switch_expression->size[1] == 8) {
17749 b_kstr = 1;
17750 do {
17751 exitg1 = 0;
17752 if (b_kstr - 1 < 8) {
17753 loop_ub = b_kstr - 1;
17754 if (switch_expression->data[loop_ub] != b[loop_ub]) {
17755 exitg1 = 1;
17756 } else {
17757 b_kstr++;
17758 }
17759 } else {
17760 b_bool = true;
17761 exitg1 = 1;
17762 }
17763 } while (exitg1 == 0);
17764 }
17765
17766 if (b_bool) {
17767 b_kstr = 0;
17768 } else {
17769 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17770 b_0[b_kstr] = tmp_5[b_kstr];
17771 }
17772
17773 b_bool = false;
17774 if (switch_expression->size[1] == 9) {
17775 b_kstr = 1;
17776 do {
17777 exitg1 = 0;
17778 if (b_kstr - 1 < 9) {
17779 loop_ub = b_kstr - 1;
17780 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
17781 exitg1 = 1;
17782 } else {
17783 b_kstr++;
17784 }
17785 } else {
17786 b_bool = true;
17787 exitg1 = 1;
17788 }
17789 } while (exitg1 == 0);
17790 }
17791
17792 if (b_bool) {
17793 b_kstr = 1;
17794 } else {
17795 b_kstr = -1;
17796 }
17797 }
17798
17799 cartesian_traj_emxFree_char_T_a(&switch_expression);
17800 switch (b_kstr) {
17801 case 0:
17802 tmp[0] = 0;
17803 tmp[1] = 0;
17804 tmp[2] = 1;
17805 tmp[3] = 0;
17806 tmp[4] = 0;
17807 tmp[5] = 0;
17808 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17809 msubspace_data[b_kstr] = tmp[b_kstr];
17810 }
17811
17812 poslim_data[0] = -3.1415926535897931;
17813 poslim_data[1] = 3.1415926535897931;
17814 iobj_0->VelocityNumber = 1.0;
17815 iobj_0->PositionNumber = 1.0;
17816 iobj_0->JointAxisInternal[0] = 0.0;
17817 iobj_0->JointAxisInternal[1] = 0.0;
17818 iobj_0->JointAxisInternal[2] = 1.0;
17819 break;
17820
17821 case 1:
17822 tmp[0] = 0;
17823 tmp[1] = 0;
17824 tmp[2] = 0;
17825 tmp[3] = 0;
17826 tmp[4] = 0;
17827 tmp[5] = 1;
17828 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17829 msubspace_data[b_kstr] = tmp[b_kstr];
17830 }
17831
17832 poslim_data[0] = -0.5;
17833 poslim_data[1] = 0.5;
17834 iobj_0->VelocityNumber = 1.0;
17835 iobj_0->PositionNumber = 1.0;
17836 iobj_0->JointAxisInternal[0] = 0.0;
17837 iobj_0->JointAxisInternal[1] = 0.0;
17838 iobj_0->JointAxisInternal[2] = 1.0;
17839 break;
17840
17841 default:
17842 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17843 msubspace_data[b_kstr] = 0;
17844 }
17845
17846 poslim_data[0] = 0.0;
17847 poslim_data[1] = 0.0;
17848 iobj_0->VelocityNumber = 0.0;
17849 iobj_0->PositionNumber = 0.0;
17850 iobj_0->JointAxisInternal[0] = 0.0;
17851 iobj_0->JointAxisInternal[1] = 0.0;
17852 iobj_0->JointAxisInternal[2] = 0.0;
17853 break;
17854 }
17855
17856 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
17857 iobj_0->MotionSubspace->size[0] = 6;
17858 iobj_0->MotionSubspace->size[1] = 1;
17859 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
17860 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17861 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
17862 }
17863
17864 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
17865 iobj_0->PositionLimitsInternal->size[1];
17866 iobj_0->PositionLimitsInternal->size[0] = 1;
17867 iobj_0->PositionLimitsInternal->size[1] = 2;
17868 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
17869 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
17870 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
17871 }
17872
17873 b_kstr = iobj_0->HomePositionInternal->size[0];
17874 iobj_0->HomePositionInternal->size[0] = 1;
17875 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
17876 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
17877 iobj_0->HomePositionInternal->data[0] = 0.0;
17878 }
17879
17880 obj->JointInternal = iobj_0;
17881 obj->Index = -1.0;
17882 obj->ParentIndex = -1.0;
17883 return b_obj;
17884}
17885
17886static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
17887 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
17888 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
17889 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
17890 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
17891 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_as_T
17892 *iobj_7, c_rigidBodyJoint_cartesian_as_T *iobj_8,
17893 c_rigidBodyJoint_cartesian_as_T *iobj_9, c_rigidBodyJoint_cartesian_as_T
17894 *iobj_10, c_rigidBodyJoint_cartesian_as_T *iobj_11,
17895 c_rigidBodyJoint_cartesian_as_T *iobj_12, c_rigidBodyJoint_cartesian_as_T
17896 *iobj_13, c_rigidBodyJoint_cartesian_as_T *iobj_14,
17897 w_robotics_manip_internal_Rig_T *iobj_15)
17898{
17899 emxArray_char_T_cartesian_tra_T *switch_expression;
17900 static const char_T tmp[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17901 '\x06' };
17902
17903 static const int8_T tmp_0[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
17904 1 };
17905
17906 static const char_T tmp_1[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17907 '\x06', '_', 'j', 'n', 't' };
17908
17909 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
17910
17911 static const char_T tmp_3[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17912
17913 static const char_T tmp_4[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17914
17915 static const char_T tmp_5[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17916 '\x07' };
17917
17918 static const char_T tmp_6[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17919 '\x07', '_', 'j', 'n', 't' };
17920
17921 static const char_T tmp_7[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17922 '\x08' };
17923
17924 static const char_T tmp_8[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
17925 '\x08', '_', 'j', 'n', 't' };
17926
17927 int32_T exitg1;
17928 obj->Bodies[0] = c_RigidBody_Rigid_i(iobj_15, iobj_14);
17929 obj->Bodies[1] = c_RigidBody_Rigid_l(iobj_0, iobj_7);
17930 obj->Bodies[2] = c_RigidBody_Rigid_lh(iobj_1, iobj_8);
17931 obj->Bodies[3] = c_RigidBody_Rigid_k(iobj_2, iobj_9);
17932 obj->Bodies[4] = c_RigidBody_Rigid_h(iobj_3, iobj_10);
17933 cartesian_trajectory_planner_B.b_kstr_h = iobj_4->NameInternal->size[0] *
17934 iobj_4->NameInternal->size[1];
17935 iobj_4->NameInternal->size[0] = 1;
17936 iobj_4->NameInternal->size[1] = 10;
17937 cart_emxEnsureCapacity_char_T_a(iobj_4->NameInternal,
17938 cartesian_trajectory_planner_B.b_kstr_h);
17939 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
17940 cartesian_trajectory_planner_B.b_kstr_h < 10;
17941 cartesian_trajectory_planner_B.b_kstr_h++) {
17942 iobj_4->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_h] =
17943 tmp[cartesian_trajectory_planner_B.b_kstr_h];
17944 }
17945
17946 iobj_11->InTree = false;
17947 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
17948 cartesian_trajectory_planner_B.b_kstr_h < 16;
17949 cartesian_trajectory_planner_B.b_kstr_h++) {
17950 iobj_11->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_h] =
17951 tmp_0[cartesian_trajectory_planner_B.b_kstr_h];
17952 }
17953
17954 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
17955 cartesian_trajectory_planner_B.b_kstr_h < 16;
17956 cartesian_trajectory_planner_B.b_kstr_h++) {
17957 iobj_11->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_h] =
17958 tmp_0[cartesian_trajectory_planner_B.b_kstr_h];
17959 }
17960
17961 cartesian_trajectory_planner_B.b_kstr_h = iobj_11->NameInternal->size[0] *
17962 iobj_11->NameInternal->size[1];
17963 iobj_11->NameInternal->size[0] = 1;
17964 iobj_11->NameInternal->size[1] = 14;
17965 cart_emxEnsureCapacity_char_T_a(iobj_11->NameInternal,
17966 cartesian_trajectory_planner_B.b_kstr_h);
17967 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
17968 cartesian_trajectory_planner_B.b_kstr_h < 14;
17969 cartesian_trajectory_planner_B.b_kstr_h++) {
17970 iobj_11->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_h] =
17971 tmp_1[cartesian_trajectory_planner_B.b_kstr_h];
17972 }
17973
17974 cartesian_trajectory_planner_B.b_kstr_h = iobj_11->Type->size[0] *
17975 iobj_11->Type->size[1];
17976 iobj_11->Type->size[0] = 1;
17977 iobj_11->Type->size[1] = 5;
17978 cart_emxEnsureCapacity_char_T_a(iobj_11->Type,
17979 cartesian_trajectory_planner_B.b_kstr_h);
17980 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
17981 cartesian_trajectory_planner_B.b_kstr_h < 5;
17982 cartesian_trajectory_planner_B.b_kstr_h++) {
17983 iobj_11->Type->data[cartesian_trajectory_planner_B.b_kstr_h] =
17984 tmp_2[cartesian_trajectory_planner_B.b_kstr_h];
17985 }
17986
17987 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
17988 cartesian_trajectory_planner_B.b_kstr_h = switch_expression->size[0] *
17989 switch_expression->size[1];
17990 switch_expression->size[0] = 1;
17991 switch_expression->size[1] = iobj_11->Type->size[1];
17992 cart_emxEnsureCapacity_char_T_a(switch_expression,
17993 cartesian_trajectory_planner_B.b_kstr_h);
17994 cartesian_trajectory_planner_B.loop_ub_k = iobj_11->Type->size[0] *
17995 iobj_11->Type->size[1] - 1;
17996 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
17997 cartesian_trajectory_planner_B.b_kstr_h <=
17998 cartesian_trajectory_planner_B.loop_ub_k;
17999 cartesian_trajectory_planner_B.b_kstr_h++) {
18000 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_h] =
18001 iobj_11->Type->data[cartesian_trajectory_planner_B.b_kstr_h];
18002 }
18003
18004 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18005 cartesian_trajectory_planner_B.b_kstr_h < 8;
18006 cartesian_trajectory_planner_B.b_kstr_h++) {
18007 cartesian_trajectory_planner_B.b_p2[cartesian_trajectory_planner_B.b_kstr_h]
18008 = tmp_3[cartesian_trajectory_planner_B.b_kstr_h];
18009 }
18010
18011 cartesian_trajectory_planner_B.b_bool_j = false;
18012 if (switch_expression->size[1] == 8) {
18013 cartesian_trajectory_planner_B.b_kstr_h = 1;
18014 do {
18015 exitg1 = 0;
18016 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 8) {
18017 cartesian_trajectory_planner_B.loop_ub_k =
18018 cartesian_trajectory_planner_B.b_kstr_h - 1;
18019 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_k] !=
18020 cartesian_trajectory_planner_B.b_p2[cartesian_trajectory_planner_B.loop_ub_k])
18021 {
18022 exitg1 = 1;
18023 } else {
18024 cartesian_trajectory_planner_B.b_kstr_h++;
18025 }
18026 } else {
18027 cartesian_trajectory_planner_B.b_bool_j = true;
18028 exitg1 = 1;
18029 }
18030 } while (exitg1 == 0);
18031 }
18032
18033 if (cartesian_trajectory_planner_B.b_bool_j) {
18034 cartesian_trajectory_planner_B.b_kstr_h = 0;
18035 } else {
18036 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18037 cartesian_trajectory_planner_B.b_kstr_h < 9;
18038 cartesian_trajectory_planner_B.b_kstr_h++) {
18039 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.b_kstr_h]
18040 = tmp_4[cartesian_trajectory_planner_B.b_kstr_h];
18041 }
18042
18043 cartesian_trajectory_planner_B.b_bool_j = false;
18044 if (switch_expression->size[1] == 9) {
18045 cartesian_trajectory_planner_B.b_kstr_h = 1;
18046 do {
18047 exitg1 = 0;
18048 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 9) {
18049 cartesian_trajectory_planner_B.loop_ub_k =
18050 cartesian_trajectory_planner_B.b_kstr_h - 1;
18051 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_k]
18052 !=
18053 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.loop_ub_k])
18054 {
18055 exitg1 = 1;
18056 } else {
18057 cartesian_trajectory_planner_B.b_kstr_h++;
18058 }
18059 } else {
18060 cartesian_trajectory_planner_B.b_bool_j = true;
18061 exitg1 = 1;
18062 }
18063 } while (exitg1 == 0);
18064 }
18065
18066 if (cartesian_trajectory_planner_B.b_bool_j) {
18067 cartesian_trajectory_planner_B.b_kstr_h = 1;
18068 } else {
18069 cartesian_trajectory_planner_B.b_kstr_h = -1;
18070 }
18071 }
18072
18073 switch (cartesian_trajectory_planner_B.b_kstr_h) {
18074 case 0:
18075 cartesian_trajectory_planner_B.iv1[0] = 0;
18076 cartesian_trajectory_planner_B.iv1[1] = 0;
18077 cartesian_trajectory_planner_B.iv1[2] = 1;
18078 cartesian_trajectory_planner_B.iv1[3] = 0;
18079 cartesian_trajectory_planner_B.iv1[4] = 0;
18080 cartesian_trajectory_planner_B.iv1[5] = 0;
18081 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18082 cartesian_trajectory_planner_B.b_kstr_h < 6;
18083 cartesian_trajectory_planner_B.b_kstr_h++) {
18084 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h]
18085 =
18086 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_h];
18087 }
18088
18089 cartesian_trajectory_planner_B.poslim_data_a[0] = -3.1415926535897931;
18090 cartesian_trajectory_planner_B.poslim_data_a[1] = 3.1415926535897931;
18091 iobj_11->VelocityNumber = 1.0;
18092 iobj_11->PositionNumber = 1.0;
18093 iobj_11->JointAxisInternal[0] = 0.0;
18094 iobj_11->JointAxisInternal[1] = 0.0;
18095 iobj_11->JointAxisInternal[2] = 1.0;
18096 break;
18097
18098 case 1:
18099 cartesian_trajectory_planner_B.iv1[0] = 0;
18100 cartesian_trajectory_planner_B.iv1[1] = 0;
18101 cartesian_trajectory_planner_B.iv1[2] = 0;
18102 cartesian_trajectory_planner_B.iv1[3] = 0;
18103 cartesian_trajectory_planner_B.iv1[4] = 0;
18104 cartesian_trajectory_planner_B.iv1[5] = 1;
18105 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18106 cartesian_trajectory_planner_B.b_kstr_h < 6;
18107 cartesian_trajectory_planner_B.b_kstr_h++) {
18108 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h]
18109 =
18110 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_h];
18111 }
18112
18113 cartesian_trajectory_planner_B.poslim_data_a[0] = -0.5;
18114 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.5;
18115 iobj_11->VelocityNumber = 1.0;
18116 iobj_11->PositionNumber = 1.0;
18117 iobj_11->JointAxisInternal[0] = 0.0;
18118 iobj_11->JointAxisInternal[1] = 0.0;
18119 iobj_11->JointAxisInternal[2] = 1.0;
18120 break;
18121
18122 default:
18123 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18124 cartesian_trajectory_planner_B.b_kstr_h < 6;
18125 cartesian_trajectory_planner_B.b_kstr_h++) {
18126 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h]
18127 = 0;
18128 }
18129
18130 cartesian_trajectory_planner_B.poslim_data_a[0] = 0.0;
18131 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.0;
18132 iobj_11->VelocityNumber = 0.0;
18133 iobj_11->PositionNumber = 0.0;
18134 iobj_11->JointAxisInternal[0] = 0.0;
18135 iobj_11->JointAxisInternal[1] = 0.0;
18136 iobj_11->JointAxisInternal[2] = 0.0;
18137 break;
18138 }
18139
18140 cartesian_trajectory_planner_B.b_kstr_h = iobj_11->MotionSubspace->size[0] *
18141 iobj_11->MotionSubspace->size[1];
18142 iobj_11->MotionSubspace->size[0] = 6;
18143 iobj_11->MotionSubspace->size[1] = 1;
18144 cartes_emxEnsureCapacity_real_T(iobj_11->MotionSubspace,
18145 cartesian_trajectory_planner_B.b_kstr_h);
18146 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18147 cartesian_trajectory_planner_B.b_kstr_h < 6;
18148 cartesian_trajectory_planner_B.b_kstr_h++) {
18149 iobj_11->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_h] =
18150 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h];
18151 }
18152
18153 cartesian_trajectory_planner_B.b_kstr_h = iobj_11->
18154 PositionLimitsInternal->size[0] * iobj_11->PositionLimitsInternal->size[1];
18155 iobj_11->PositionLimitsInternal->size[0] = 1;
18156 iobj_11->PositionLimitsInternal->size[1] = 2;
18157 cartes_emxEnsureCapacity_real_T(iobj_11->PositionLimitsInternal,
18158 cartesian_trajectory_planner_B.b_kstr_h);
18159 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18160 cartesian_trajectory_planner_B.b_kstr_h < 2;
18161 cartesian_trajectory_planner_B.b_kstr_h++) {
18162 iobj_11->PositionLimitsInternal->
18163 data[cartesian_trajectory_planner_B.b_kstr_h] =
18164 cartesian_trajectory_planner_B.poslim_data_a[cartesian_trajectory_planner_B.b_kstr_h];
18165 }
18166
18167 cartesian_trajectory_planner_B.b_kstr_h = iobj_11->HomePositionInternal->size
18168 [0];
18169 iobj_11->HomePositionInternal->size[0] = 1;
18170 cartes_emxEnsureCapacity_real_T(iobj_11->HomePositionInternal,
18171 cartesian_trajectory_planner_B.b_kstr_h);
18172 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18173 cartesian_trajectory_planner_B.b_kstr_h < 1;
18174 cartesian_trajectory_planner_B.b_kstr_h++) {
18175 iobj_11->HomePositionInternal->data[0] = 0.0;
18176 }
18177
18178 iobj_4->JointInternal = iobj_11;
18179 iobj_4->Index = -1.0;
18180 iobj_4->ParentIndex = -1.0;
18181 obj->Bodies[5] = iobj_4;
18182 cartesian_trajectory_planner_B.b_kstr_h = iobj_5->NameInternal->size[0] *
18183 iobj_5->NameInternal->size[1];
18184 iobj_5->NameInternal->size[0] = 1;
18185 iobj_5->NameInternal->size[1] = 10;
18186 cart_emxEnsureCapacity_char_T_a(iobj_5->NameInternal,
18187 cartesian_trajectory_planner_B.b_kstr_h);
18188 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18189 cartesian_trajectory_planner_B.b_kstr_h < 10;
18190 cartesian_trajectory_planner_B.b_kstr_h++) {
18191 iobj_5->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_h] =
18192 tmp_5[cartesian_trajectory_planner_B.b_kstr_h];
18193 }
18194
18195 iobj_12->InTree = false;
18196 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18197 cartesian_trajectory_planner_B.b_kstr_h < 16;
18198 cartesian_trajectory_planner_B.b_kstr_h++) {
18199 iobj_12->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_h] =
18200 tmp_0[cartesian_trajectory_planner_B.b_kstr_h];
18201 }
18202
18203 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18204 cartesian_trajectory_planner_B.b_kstr_h < 16;
18205 cartesian_trajectory_planner_B.b_kstr_h++) {
18206 iobj_12->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_h] =
18207 tmp_0[cartesian_trajectory_planner_B.b_kstr_h];
18208 }
18209
18210 cartesian_trajectory_planner_B.b_kstr_h = iobj_12->NameInternal->size[0] *
18211 iobj_12->NameInternal->size[1];
18212 iobj_12->NameInternal->size[0] = 1;
18213 iobj_12->NameInternal->size[1] = 14;
18214 cart_emxEnsureCapacity_char_T_a(iobj_12->NameInternal,
18215 cartesian_trajectory_planner_B.b_kstr_h);
18216 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18217 cartesian_trajectory_planner_B.b_kstr_h < 14;
18218 cartesian_trajectory_planner_B.b_kstr_h++) {
18219 iobj_12->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_h] =
18220 tmp_6[cartesian_trajectory_planner_B.b_kstr_h];
18221 }
18222
18223 cartesian_trajectory_planner_B.b_kstr_h = iobj_12->Type->size[0] *
18224 iobj_12->Type->size[1];
18225 iobj_12->Type->size[0] = 1;
18226 iobj_12->Type->size[1] = 5;
18227 cart_emxEnsureCapacity_char_T_a(iobj_12->Type,
18228 cartesian_trajectory_planner_B.b_kstr_h);
18229 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18230 cartesian_trajectory_planner_B.b_kstr_h < 5;
18231 cartesian_trajectory_planner_B.b_kstr_h++) {
18232 iobj_12->Type->data[cartesian_trajectory_planner_B.b_kstr_h] =
18233 tmp_2[cartesian_trajectory_planner_B.b_kstr_h];
18234 }
18235
18236 cartesian_trajectory_planner_B.b_kstr_h = switch_expression->size[0] *
18237 switch_expression->size[1];
18238 switch_expression->size[0] = 1;
18239 switch_expression->size[1] = iobj_12->Type->size[1];
18240 cart_emxEnsureCapacity_char_T_a(switch_expression,
18241 cartesian_trajectory_planner_B.b_kstr_h);
18242 cartesian_trajectory_planner_B.loop_ub_k = iobj_12->Type->size[0] *
18243 iobj_12->Type->size[1] - 1;
18244 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18245 cartesian_trajectory_planner_B.b_kstr_h <=
18246 cartesian_trajectory_planner_B.loop_ub_k;
18247 cartesian_trajectory_planner_B.b_kstr_h++) {
18248 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_h] =
18249 iobj_12->Type->data[cartesian_trajectory_planner_B.b_kstr_h];
18250 }
18251
18252 cartesian_trajectory_planner_B.b_bool_j = false;
18253 if (switch_expression->size[1] == 8) {
18254 cartesian_trajectory_planner_B.b_kstr_h = 1;
18255 do {
18256 exitg1 = 0;
18257 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 8) {
18258 cartesian_trajectory_planner_B.loop_ub_k =
18259 cartesian_trajectory_planner_B.b_kstr_h - 1;
18260 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_k] !=
18261 cartesian_trajectory_planner_B.b_p2[cartesian_trajectory_planner_B.loop_ub_k])
18262 {
18263 exitg1 = 1;
18264 } else {
18265 cartesian_trajectory_planner_B.b_kstr_h++;
18266 }
18267 } else {
18268 cartesian_trajectory_planner_B.b_bool_j = true;
18269 exitg1 = 1;
18270 }
18271 } while (exitg1 == 0);
18272 }
18273
18274 if (cartesian_trajectory_planner_B.b_bool_j) {
18275 cartesian_trajectory_planner_B.b_kstr_h = 0;
18276 } else {
18277 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18278 cartesian_trajectory_planner_B.b_kstr_h < 9;
18279 cartesian_trajectory_planner_B.b_kstr_h++) {
18280 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.b_kstr_h]
18281 = tmp_4[cartesian_trajectory_planner_B.b_kstr_h];
18282 }
18283
18284 cartesian_trajectory_planner_B.b_bool_j = false;
18285 if (switch_expression->size[1] == 9) {
18286 cartesian_trajectory_planner_B.b_kstr_h = 1;
18287 do {
18288 exitg1 = 0;
18289 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 9) {
18290 cartesian_trajectory_planner_B.loop_ub_k =
18291 cartesian_trajectory_planner_B.b_kstr_h - 1;
18292 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_k]
18293 !=
18294 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.loop_ub_k])
18295 {
18296 exitg1 = 1;
18297 } else {
18298 cartesian_trajectory_planner_B.b_kstr_h++;
18299 }
18300 } else {
18301 cartesian_trajectory_planner_B.b_bool_j = true;
18302 exitg1 = 1;
18303 }
18304 } while (exitg1 == 0);
18305 }
18306
18307 if (cartesian_trajectory_planner_B.b_bool_j) {
18308 cartesian_trajectory_planner_B.b_kstr_h = 1;
18309 } else {
18310 cartesian_trajectory_planner_B.b_kstr_h = -1;
18311 }
18312 }
18313
18314 switch (cartesian_trajectory_planner_B.b_kstr_h) {
18315 case 0:
18316 cartesian_trajectory_planner_B.iv1[0] = 0;
18317 cartesian_trajectory_planner_B.iv1[1] = 0;
18318 cartesian_trajectory_planner_B.iv1[2] = 1;
18319 cartesian_trajectory_planner_B.iv1[3] = 0;
18320 cartesian_trajectory_planner_B.iv1[4] = 0;
18321 cartesian_trajectory_planner_B.iv1[5] = 0;
18322 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18323 cartesian_trajectory_planner_B.b_kstr_h < 6;
18324 cartesian_trajectory_planner_B.b_kstr_h++) {
18325 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h]
18326 =
18327 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_h];
18328 }
18329
18330 cartesian_trajectory_planner_B.poslim_data_a[0] = -3.1415926535897931;
18331 cartesian_trajectory_planner_B.poslim_data_a[1] = 3.1415926535897931;
18332 iobj_12->VelocityNumber = 1.0;
18333 iobj_12->PositionNumber = 1.0;
18334 iobj_12->JointAxisInternal[0] = 0.0;
18335 iobj_12->JointAxisInternal[1] = 0.0;
18336 iobj_12->JointAxisInternal[2] = 1.0;
18337 break;
18338
18339 case 1:
18340 cartesian_trajectory_planner_B.iv1[0] = 0;
18341 cartesian_trajectory_planner_B.iv1[1] = 0;
18342 cartesian_trajectory_planner_B.iv1[2] = 0;
18343 cartesian_trajectory_planner_B.iv1[3] = 0;
18344 cartesian_trajectory_planner_B.iv1[4] = 0;
18345 cartesian_trajectory_planner_B.iv1[5] = 1;
18346 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18347 cartesian_trajectory_planner_B.b_kstr_h < 6;
18348 cartesian_trajectory_planner_B.b_kstr_h++) {
18349 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h]
18350 =
18351 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_h];
18352 }
18353
18354 cartesian_trajectory_planner_B.poslim_data_a[0] = -0.5;
18355 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.5;
18356 iobj_12->VelocityNumber = 1.0;
18357 iobj_12->PositionNumber = 1.0;
18358 iobj_12->JointAxisInternal[0] = 0.0;
18359 iobj_12->JointAxisInternal[1] = 0.0;
18360 iobj_12->JointAxisInternal[2] = 1.0;
18361 break;
18362
18363 default:
18364 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18365 cartesian_trajectory_planner_B.b_kstr_h < 6;
18366 cartesian_trajectory_planner_B.b_kstr_h++) {
18367 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h]
18368 = 0;
18369 }
18370
18371 cartesian_trajectory_planner_B.poslim_data_a[0] = 0.0;
18372 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.0;
18373 iobj_12->VelocityNumber = 0.0;
18374 iobj_12->PositionNumber = 0.0;
18375 iobj_12->JointAxisInternal[0] = 0.0;
18376 iobj_12->JointAxisInternal[1] = 0.0;
18377 iobj_12->JointAxisInternal[2] = 0.0;
18378 break;
18379 }
18380
18381 cartesian_trajectory_planner_B.b_kstr_h = iobj_12->MotionSubspace->size[0] *
18382 iobj_12->MotionSubspace->size[1];
18383 iobj_12->MotionSubspace->size[0] = 6;
18384 iobj_12->MotionSubspace->size[1] = 1;
18385 cartes_emxEnsureCapacity_real_T(iobj_12->MotionSubspace,
18386 cartesian_trajectory_planner_B.b_kstr_h);
18387 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18388 cartesian_trajectory_planner_B.b_kstr_h < 6;
18389 cartesian_trajectory_planner_B.b_kstr_h++) {
18390 iobj_12->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_h] =
18391 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h];
18392 }
18393
18394 cartesian_trajectory_planner_B.b_kstr_h = iobj_12->
18395 PositionLimitsInternal->size[0] * iobj_12->PositionLimitsInternal->size[1];
18396 iobj_12->PositionLimitsInternal->size[0] = 1;
18397 iobj_12->PositionLimitsInternal->size[1] = 2;
18398 cartes_emxEnsureCapacity_real_T(iobj_12->PositionLimitsInternal,
18399 cartesian_trajectory_planner_B.b_kstr_h);
18400 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18401 cartesian_trajectory_planner_B.b_kstr_h < 2;
18402 cartesian_trajectory_planner_B.b_kstr_h++) {
18403 iobj_12->PositionLimitsInternal->
18404 data[cartesian_trajectory_planner_B.b_kstr_h] =
18405 cartesian_trajectory_planner_B.poslim_data_a[cartesian_trajectory_planner_B.b_kstr_h];
18406 }
18407
18408 cartesian_trajectory_planner_B.b_kstr_h = iobj_12->HomePositionInternal->size
18409 [0];
18410 iobj_12->HomePositionInternal->size[0] = 1;
18411 cartes_emxEnsureCapacity_real_T(iobj_12->HomePositionInternal,
18412 cartesian_trajectory_planner_B.b_kstr_h);
18413 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18414 cartesian_trajectory_planner_B.b_kstr_h < 1;
18415 cartesian_trajectory_planner_B.b_kstr_h++) {
18416 iobj_12->HomePositionInternal->data[0] = 0.0;
18417 }
18418
18419 iobj_5->JointInternal = iobj_12;
18420 iobj_5->Index = -1.0;
18421 iobj_5->ParentIndex = -1.0;
18422 obj->Bodies[6] = iobj_5;
18423 cartesian_trajectory_planner_B.b_kstr_h = iobj_6->NameInternal->size[0] *
18424 iobj_6->NameInternal->size[1];
18425 iobj_6->NameInternal->size[0] = 1;
18426 iobj_6->NameInternal->size[1] = 10;
18427 cart_emxEnsureCapacity_char_T_a(iobj_6->NameInternal,
18428 cartesian_trajectory_planner_B.b_kstr_h);
18429 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18430 cartesian_trajectory_planner_B.b_kstr_h < 10;
18431 cartesian_trajectory_planner_B.b_kstr_h++) {
18432 iobj_6->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_h] =
18433 tmp_7[cartesian_trajectory_planner_B.b_kstr_h];
18434 }
18435
18436 iobj_13->InTree = false;
18437 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18438 cartesian_trajectory_planner_B.b_kstr_h < 16;
18439 cartesian_trajectory_planner_B.b_kstr_h++) {
18440 iobj_13->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_h] =
18441 tmp_0[cartesian_trajectory_planner_B.b_kstr_h];
18442 }
18443
18444 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18445 cartesian_trajectory_planner_B.b_kstr_h < 16;
18446 cartesian_trajectory_planner_B.b_kstr_h++) {
18447 iobj_13->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_h] =
18448 tmp_0[cartesian_trajectory_planner_B.b_kstr_h];
18449 }
18450
18451 cartesian_trajectory_planner_B.b_kstr_h = iobj_13->NameInternal->size[0] *
18452 iobj_13->NameInternal->size[1];
18453 iobj_13->NameInternal->size[0] = 1;
18454 iobj_13->NameInternal->size[1] = 14;
18455 cart_emxEnsureCapacity_char_T_a(iobj_13->NameInternal,
18456 cartesian_trajectory_planner_B.b_kstr_h);
18457 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18458 cartesian_trajectory_planner_B.b_kstr_h < 14;
18459 cartesian_trajectory_planner_B.b_kstr_h++) {
18460 iobj_13->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_h] =
18461 tmp_8[cartesian_trajectory_planner_B.b_kstr_h];
18462 }
18463
18464 cartesian_trajectory_planner_B.b_kstr_h = iobj_13->Type->size[0] *
18465 iobj_13->Type->size[1];
18466 iobj_13->Type->size[0] = 1;
18467 iobj_13->Type->size[1] = 5;
18468 cart_emxEnsureCapacity_char_T_a(iobj_13->Type,
18469 cartesian_trajectory_planner_B.b_kstr_h);
18470 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18471 cartesian_trajectory_planner_B.b_kstr_h < 5;
18472 cartesian_trajectory_planner_B.b_kstr_h++) {
18473 iobj_13->Type->data[cartesian_trajectory_planner_B.b_kstr_h] =
18474 tmp_2[cartesian_trajectory_planner_B.b_kstr_h];
18475 }
18476
18477 cartesian_trajectory_planner_B.b_kstr_h = switch_expression->size[0] *
18478 switch_expression->size[1];
18479 switch_expression->size[0] = 1;
18480 switch_expression->size[1] = iobj_13->Type->size[1];
18481 cart_emxEnsureCapacity_char_T_a(switch_expression,
18482 cartesian_trajectory_planner_B.b_kstr_h);
18483 cartesian_trajectory_planner_B.loop_ub_k = iobj_13->Type->size[0] *
18484 iobj_13->Type->size[1] - 1;
18485 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18486 cartesian_trajectory_planner_B.b_kstr_h <=
18487 cartesian_trajectory_planner_B.loop_ub_k;
18488 cartesian_trajectory_planner_B.b_kstr_h++) {
18489 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_h] =
18490 iobj_13->Type->data[cartesian_trajectory_planner_B.b_kstr_h];
18491 }
18492
18493 cartesian_trajectory_planner_B.b_bool_j = false;
18494 if (switch_expression->size[1] == 8) {
18495 cartesian_trajectory_planner_B.b_kstr_h = 1;
18496 do {
18497 exitg1 = 0;
18498 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 8) {
18499 cartesian_trajectory_planner_B.loop_ub_k =
18500 cartesian_trajectory_planner_B.b_kstr_h - 1;
18501 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_k] !=
18502 cartesian_trajectory_planner_B.b_p2[cartesian_trajectory_planner_B.loop_ub_k])
18503 {
18504 exitg1 = 1;
18505 } else {
18506 cartesian_trajectory_planner_B.b_kstr_h++;
18507 }
18508 } else {
18509 cartesian_trajectory_planner_B.b_bool_j = true;
18510 exitg1 = 1;
18511 }
18512 } while (exitg1 == 0);
18513 }
18514
18515 if (cartesian_trajectory_planner_B.b_bool_j) {
18516 cartesian_trajectory_planner_B.b_kstr_h = 0;
18517 } else {
18518 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18519 cartesian_trajectory_planner_B.b_kstr_h < 9;
18520 cartesian_trajectory_planner_B.b_kstr_h++) {
18521 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.b_kstr_h]
18522 = tmp_4[cartesian_trajectory_planner_B.b_kstr_h];
18523 }
18524
18525 cartesian_trajectory_planner_B.b_bool_j = false;
18526 if (switch_expression->size[1] == 9) {
18527 cartesian_trajectory_planner_B.b_kstr_h = 1;
18528 do {
18529 exitg1 = 0;
18530 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 9) {
18531 cartesian_trajectory_planner_B.loop_ub_k =
18532 cartesian_trajectory_planner_B.b_kstr_h - 1;
18533 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_k]
18534 !=
18535 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.loop_ub_k])
18536 {
18537 exitg1 = 1;
18538 } else {
18539 cartesian_trajectory_planner_B.b_kstr_h++;
18540 }
18541 } else {
18542 cartesian_trajectory_planner_B.b_bool_j = true;
18543 exitg1 = 1;
18544 }
18545 } while (exitg1 == 0);
18546 }
18547
18548 if (cartesian_trajectory_planner_B.b_bool_j) {
18549 cartesian_trajectory_planner_B.b_kstr_h = 1;
18550 } else {
18551 cartesian_trajectory_planner_B.b_kstr_h = -1;
18552 }
18553 }
18554
18555 cartesian_traj_emxFree_char_T_a(&switch_expression);
18556 switch (cartesian_trajectory_planner_B.b_kstr_h) {
18557 case 0:
18558 cartesian_trajectory_planner_B.iv1[0] = 0;
18559 cartesian_trajectory_planner_B.iv1[1] = 0;
18560 cartesian_trajectory_planner_B.iv1[2] = 1;
18561 cartesian_trajectory_planner_B.iv1[3] = 0;
18562 cartesian_trajectory_planner_B.iv1[4] = 0;
18563 cartesian_trajectory_planner_B.iv1[5] = 0;
18564 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18565 cartesian_trajectory_planner_B.b_kstr_h < 6;
18566 cartesian_trajectory_planner_B.b_kstr_h++) {
18567 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h]
18568 =
18569 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_h];
18570 }
18571
18572 cartesian_trajectory_planner_B.poslim_data_a[0] = -3.1415926535897931;
18573 cartesian_trajectory_planner_B.poslim_data_a[1] = 3.1415926535897931;
18574 iobj_13->VelocityNumber = 1.0;
18575 iobj_13->PositionNumber = 1.0;
18576 iobj_13->JointAxisInternal[0] = 0.0;
18577 iobj_13->JointAxisInternal[1] = 0.0;
18578 iobj_13->JointAxisInternal[2] = 1.0;
18579 break;
18580
18581 case 1:
18582 cartesian_trajectory_planner_B.iv1[0] = 0;
18583 cartesian_trajectory_planner_B.iv1[1] = 0;
18584 cartesian_trajectory_planner_B.iv1[2] = 0;
18585 cartesian_trajectory_planner_B.iv1[3] = 0;
18586 cartesian_trajectory_planner_B.iv1[4] = 0;
18587 cartesian_trajectory_planner_B.iv1[5] = 1;
18588 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18589 cartesian_trajectory_planner_B.b_kstr_h < 6;
18590 cartesian_trajectory_planner_B.b_kstr_h++) {
18591 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h]
18592 =
18593 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_h];
18594 }
18595
18596 cartesian_trajectory_planner_B.poslim_data_a[0] = -0.5;
18597 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.5;
18598 iobj_13->VelocityNumber = 1.0;
18599 iobj_13->PositionNumber = 1.0;
18600 iobj_13->JointAxisInternal[0] = 0.0;
18601 iobj_13->JointAxisInternal[1] = 0.0;
18602 iobj_13->JointAxisInternal[2] = 1.0;
18603 break;
18604
18605 default:
18606 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18607 cartesian_trajectory_planner_B.b_kstr_h < 6;
18608 cartesian_trajectory_planner_B.b_kstr_h++) {
18609 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h]
18610 = 0;
18611 }
18612
18613 cartesian_trajectory_planner_B.poslim_data_a[0] = 0.0;
18614 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.0;
18615 iobj_13->VelocityNumber = 0.0;
18616 iobj_13->PositionNumber = 0.0;
18617 iobj_13->JointAxisInternal[0] = 0.0;
18618 iobj_13->JointAxisInternal[1] = 0.0;
18619 iobj_13->JointAxisInternal[2] = 0.0;
18620 break;
18621 }
18622
18623 cartesian_trajectory_planner_B.b_kstr_h = iobj_13->MotionSubspace->size[0] *
18624 iobj_13->MotionSubspace->size[1];
18625 iobj_13->MotionSubspace->size[0] = 6;
18626 iobj_13->MotionSubspace->size[1] = 1;
18627 cartes_emxEnsureCapacity_real_T(iobj_13->MotionSubspace,
18628 cartesian_trajectory_planner_B.b_kstr_h);
18629 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18630 cartesian_trajectory_planner_B.b_kstr_h < 6;
18631 cartesian_trajectory_planner_B.b_kstr_h++) {
18632 iobj_13->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_h] =
18633 cartesian_trajectory_planner_B.msubspace_data_k[cartesian_trajectory_planner_B.b_kstr_h];
18634 }
18635
18636 cartesian_trajectory_planner_B.b_kstr_h = iobj_13->
18637 PositionLimitsInternal->size[0] * iobj_13->PositionLimitsInternal->size[1];
18638 iobj_13->PositionLimitsInternal->size[0] = 1;
18639 iobj_13->PositionLimitsInternal->size[1] = 2;
18640 cartes_emxEnsureCapacity_real_T(iobj_13->PositionLimitsInternal,
18641 cartesian_trajectory_planner_B.b_kstr_h);
18642 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18643 cartesian_trajectory_planner_B.b_kstr_h < 2;
18644 cartesian_trajectory_planner_B.b_kstr_h++) {
18645 iobj_13->PositionLimitsInternal->
18646 data[cartesian_trajectory_planner_B.b_kstr_h] =
18647 cartesian_trajectory_planner_B.poslim_data_a[cartesian_trajectory_planner_B.b_kstr_h];
18648 }
18649
18650 cartesian_trajectory_planner_B.b_kstr_h = iobj_13->HomePositionInternal->size
18651 [0];
18652 iobj_13->HomePositionInternal->size[0] = 1;
18653 cartes_emxEnsureCapacity_real_T(iobj_13->HomePositionInternal,
18654 cartesian_trajectory_planner_B.b_kstr_h);
18655 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18656 cartesian_trajectory_planner_B.b_kstr_h < 1;
18657 cartesian_trajectory_planner_B.b_kstr_h++) {
18658 iobj_13->HomePositionInternal->data[0] = 0.0;
18659 }
18660
18661 iobj_6->JointInternal = iobj_13;
18662 iobj_6->Index = -1.0;
18663 iobj_6->ParentIndex = -1.0;
18664 obj->Bodies[7] = iobj_6;
18665 obj->NumBodies = 0.0;
18666 obj->NumNonFixedBodies = 0.0;
18667 obj->PositionNumber = 0.0;
18668 obj->VelocityNumber = 0.0;
18669 cartesian_trajectory_plann_rand(cartesian_trajectory_planner_B.unusedExpr_o);
18670 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18671 cartesian_trajectory_planner_B.b_kstr_h < 8;
18672 cartesian_trajectory_planner_B.b_kstr_h++) {
18673 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_h] = 0.0;
18674 }
18675
18676 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18677 cartesian_trajectory_planner_B.b_kstr_h < 8;
18678 cartesian_trajectory_planner_B.b_kstr_h++) {
18679 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_h + 8] = -1.0;
18680 }
18681
18682 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18683 cartesian_trajectory_planner_B.b_kstr_h < 8;
18684 cartesian_trajectory_planner_B.b_kstr_h++) {
18685 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_h] = 0.0;
18686 }
18687
18688 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
18689 cartesian_trajectory_planner_B.b_kstr_h < 8;
18690 cartesian_trajectory_planner_B.b_kstr_h++) {
18691 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_h + 8] = -1.0;
18692 }
18693}
18694
18695static x_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ast
18696 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
18697 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
18698 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
18699 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
18700 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
18701 c_rigidBodyJoint_cartesian_as_T *iobj_7, c_rigidBodyJoint_cartesian_as_T
18702 *iobj_8, c_rigidBodyJoint_cartesian_as_T *iobj_9,
18703 c_rigidBodyJoint_cartesian_as_T *iobj_10, c_rigidBodyJoint_cartesian_as_T
18704 *iobj_11, c_rigidBodyJoint_cartesian_as_T *iobj_12,
18705 c_rigidBodyJoint_cartesian_as_T *iobj_13, c_rigidBodyJoint_cartesian_as_T
18706 *iobj_14, c_rigidBodyJoint_cartesian_as_T *iobj_15,
18707 w_robotics_manip_internal_Rig_T *iobj_16)
18708{
18709 x_robotics_manip_internal_Rig_T *b_obj;
18710 emxArray_char_T_cartesian_tra_T *switch_expression;
18711 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
18712 };
18713
18714 static const char_T tmp_0[8] = { 'b', 'a', 's', 'e', '_', 'j', 'n', 't' };
18715
18716 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
18717
18718 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18719
18720 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18721
18722 int32_T exitg1;
18723 b_obj = obj;
18724 cartesian_trajectory_planner_B.b_kstr_c = obj->Base.NameInternal->size[0] *
18725 obj->Base.NameInternal->size[1];
18726 obj->Base.NameInternal->size[0] = 1;
18727 obj->Base.NameInternal->size[1] = 4;
18728 cart_emxEnsureCapacity_char_T_a(obj->Base.NameInternal,
18729 cartesian_trajectory_planner_B.b_kstr_c);
18730 obj->Base.NameInternal->data[0] = 'b';
18731 obj->Base.NameInternal->data[1] = 'a';
18732 obj->Base.NameInternal->data[2] = 's';
18733 obj->Base.NameInternal->data[3] = 'e';
18734 iobj_15->InTree = false;
18735 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18736 cartesian_trajectory_planner_B.b_kstr_c < 16;
18737 cartesian_trajectory_planner_B.b_kstr_c++) {
18738 iobj_15->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_c] =
18739 tmp[cartesian_trajectory_planner_B.b_kstr_c];
18740 }
18741
18742 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18743 cartesian_trajectory_planner_B.b_kstr_c < 16;
18744 cartesian_trajectory_planner_B.b_kstr_c++) {
18745 iobj_15->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_c] =
18746 tmp[cartesian_trajectory_planner_B.b_kstr_c];
18747 }
18748
18749 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->NameInternal->size[0] *
18750 iobj_15->NameInternal->size[1];
18751 iobj_15->NameInternal->size[0] = 1;
18752 iobj_15->NameInternal->size[1] = 8;
18753 cart_emxEnsureCapacity_char_T_a(iobj_15->NameInternal,
18754 cartesian_trajectory_planner_B.b_kstr_c);
18755 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18756 cartesian_trajectory_planner_B.b_kstr_c < 8;
18757 cartesian_trajectory_planner_B.b_kstr_c++) {
18758 iobj_15->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_c] =
18759 tmp_0[cartesian_trajectory_planner_B.b_kstr_c];
18760 }
18761
18762 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->Type->size[0] *
18763 iobj_15->Type->size[1];
18764 iobj_15->Type->size[0] = 1;
18765 iobj_15->Type->size[1] = 5;
18766 cart_emxEnsureCapacity_char_T_a(iobj_15->Type,
18767 cartesian_trajectory_planner_B.b_kstr_c);
18768 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18769 cartesian_trajectory_planner_B.b_kstr_c < 5;
18770 cartesian_trajectory_planner_B.b_kstr_c++) {
18771 iobj_15->Type->data[cartesian_trajectory_planner_B.b_kstr_c] =
18772 tmp_1[cartesian_trajectory_planner_B.b_kstr_c];
18773 }
18774
18775 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
18776 cartesian_trajectory_planner_B.b_kstr_c = switch_expression->size[0] *
18777 switch_expression->size[1];
18778 switch_expression->size[0] = 1;
18779 switch_expression->size[1] = iobj_15->Type->size[1];
18780 cart_emxEnsureCapacity_char_T_a(switch_expression,
18781 cartesian_trajectory_planner_B.b_kstr_c);
18782 cartesian_trajectory_planner_B.loop_ub_nn = iobj_15->Type->size[0] *
18783 iobj_15->Type->size[1] - 1;
18784 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18785 cartesian_trajectory_planner_B.b_kstr_c <=
18786 cartesian_trajectory_planner_B.loop_ub_nn;
18787 cartesian_trajectory_planner_B.b_kstr_c++) {
18788 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_c] =
18789 iobj_15->Type->data[cartesian_trajectory_planner_B.b_kstr_c];
18790 }
18791
18792 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18793 cartesian_trajectory_planner_B.b_kstr_c < 8;
18794 cartesian_trajectory_planner_B.b_kstr_c++) {
18795 cartesian_trajectory_planner_B.b_p[cartesian_trajectory_planner_B.b_kstr_c] =
18796 tmp_2[cartesian_trajectory_planner_B.b_kstr_c];
18797 }
18798
18799 cartesian_trajectory_planner_B.b_bool_cl = false;
18800 if (switch_expression->size[1] == 8) {
18801 cartesian_trajectory_planner_B.b_kstr_c = 1;
18802 do {
18803 exitg1 = 0;
18804 if (cartesian_trajectory_planner_B.b_kstr_c - 1 < 8) {
18805 cartesian_trajectory_planner_B.loop_ub_nn =
18806 cartesian_trajectory_planner_B.b_kstr_c - 1;
18807 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_nn]
18808 !=
18809 cartesian_trajectory_planner_B.b_p[cartesian_trajectory_planner_B.loop_ub_nn])
18810 {
18811 exitg1 = 1;
18812 } else {
18813 cartesian_trajectory_planner_B.b_kstr_c++;
18814 }
18815 } else {
18816 cartesian_trajectory_planner_B.b_bool_cl = true;
18817 exitg1 = 1;
18818 }
18819 } while (exitg1 == 0);
18820 }
18821
18822 if (cartesian_trajectory_planner_B.b_bool_cl) {
18823 cartesian_trajectory_planner_B.b_kstr_c = 0;
18824 } else {
18825 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18826 cartesian_trajectory_planner_B.b_kstr_c < 9;
18827 cartesian_trajectory_planner_B.b_kstr_c++) {
18828 cartesian_trajectory_planner_B.b_a[cartesian_trajectory_planner_B.b_kstr_c]
18829 = tmp_3[cartesian_trajectory_planner_B.b_kstr_c];
18830 }
18831
18832 cartesian_trajectory_planner_B.b_bool_cl = false;
18833 if (switch_expression->size[1] == 9) {
18834 cartesian_trajectory_planner_B.b_kstr_c = 1;
18835 do {
18836 exitg1 = 0;
18837 if (cartesian_trajectory_planner_B.b_kstr_c - 1 < 9) {
18838 cartesian_trajectory_planner_B.loop_ub_nn =
18839 cartesian_trajectory_planner_B.b_kstr_c - 1;
18840 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_nn]
18841 !=
18842 cartesian_trajectory_planner_B.b_a[cartesian_trajectory_planner_B.loop_ub_nn])
18843 {
18844 exitg1 = 1;
18845 } else {
18846 cartesian_trajectory_planner_B.b_kstr_c++;
18847 }
18848 } else {
18849 cartesian_trajectory_planner_B.b_bool_cl = true;
18850 exitg1 = 1;
18851 }
18852 } while (exitg1 == 0);
18853 }
18854
18855 if (cartesian_trajectory_planner_B.b_bool_cl) {
18856 cartesian_trajectory_planner_B.b_kstr_c = 1;
18857 } else {
18858 cartesian_trajectory_planner_B.b_kstr_c = -1;
18859 }
18860 }
18861
18862 cartesian_traj_emxFree_char_T_a(&switch_expression);
18863 switch (cartesian_trajectory_planner_B.b_kstr_c) {
18864 case 0:
18865 cartesian_trajectory_planner_B.iv[0] = 0;
18866 cartesian_trajectory_planner_B.iv[1] = 0;
18867 cartesian_trajectory_planner_B.iv[2] = 1;
18868 cartesian_trajectory_planner_B.iv[3] = 0;
18869 cartesian_trajectory_planner_B.iv[4] = 0;
18870 cartesian_trajectory_planner_B.iv[5] = 0;
18871 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18872 cartesian_trajectory_planner_B.b_kstr_c < 6;
18873 cartesian_trajectory_planner_B.b_kstr_c++) {
18874 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c]
18875 =
18876 cartesian_trajectory_planner_B.iv[cartesian_trajectory_planner_B.b_kstr_c];
18877 }
18878
18879 cartesian_trajectory_planner_B.poslim_data[0] = -3.1415926535897931;
18880 cartesian_trajectory_planner_B.poslim_data[1] = 3.1415926535897931;
18881 iobj_15->VelocityNumber = 1.0;
18882 iobj_15->PositionNumber = 1.0;
18883 iobj_15->JointAxisInternal[0] = 0.0;
18884 iobj_15->JointAxisInternal[1] = 0.0;
18885 iobj_15->JointAxisInternal[2] = 1.0;
18886 break;
18887
18888 case 1:
18889 cartesian_trajectory_planner_B.iv[0] = 0;
18890 cartesian_trajectory_planner_B.iv[1] = 0;
18891 cartesian_trajectory_planner_B.iv[2] = 0;
18892 cartesian_trajectory_planner_B.iv[3] = 0;
18893 cartesian_trajectory_planner_B.iv[4] = 0;
18894 cartesian_trajectory_planner_B.iv[5] = 1;
18895 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18896 cartesian_trajectory_planner_B.b_kstr_c < 6;
18897 cartesian_trajectory_planner_B.b_kstr_c++) {
18898 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c]
18899 =
18900 cartesian_trajectory_planner_B.iv[cartesian_trajectory_planner_B.b_kstr_c];
18901 }
18902
18903 cartesian_trajectory_planner_B.poslim_data[0] = -0.5;
18904 cartesian_trajectory_planner_B.poslim_data[1] = 0.5;
18905 iobj_15->VelocityNumber = 1.0;
18906 iobj_15->PositionNumber = 1.0;
18907 iobj_15->JointAxisInternal[0] = 0.0;
18908 iobj_15->JointAxisInternal[1] = 0.0;
18909 iobj_15->JointAxisInternal[2] = 1.0;
18910 break;
18911
18912 default:
18913 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18914 cartesian_trajectory_planner_B.b_kstr_c < 6;
18915 cartesian_trajectory_planner_B.b_kstr_c++) {
18916 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c]
18917 = 0;
18918 }
18919
18920 cartesian_trajectory_planner_B.poslim_data[0] = 0.0;
18921 cartesian_trajectory_planner_B.poslim_data[1] = 0.0;
18922 iobj_15->VelocityNumber = 0.0;
18923 iobj_15->PositionNumber = 0.0;
18924 iobj_15->JointAxisInternal[0] = 0.0;
18925 iobj_15->JointAxisInternal[1] = 0.0;
18926 iobj_15->JointAxisInternal[2] = 0.0;
18927 break;
18928 }
18929
18930 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->MotionSubspace->size[0] *
18931 iobj_15->MotionSubspace->size[1];
18932 iobj_15->MotionSubspace->size[0] = 6;
18933 iobj_15->MotionSubspace->size[1] = 1;
18934 cartes_emxEnsureCapacity_real_T(iobj_15->MotionSubspace,
18935 cartesian_trajectory_planner_B.b_kstr_c);
18936 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18937 cartesian_trajectory_planner_B.b_kstr_c < 6;
18938 cartesian_trajectory_planner_B.b_kstr_c++) {
18939 iobj_15->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_c] =
18940 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c];
18941 }
18942
18943 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->
18944 PositionLimitsInternal->size[0] * iobj_15->PositionLimitsInternal->size[1];
18945 iobj_15->PositionLimitsInternal->size[0] = 1;
18946 iobj_15->PositionLimitsInternal->size[1] = 2;
18947 cartes_emxEnsureCapacity_real_T(iobj_15->PositionLimitsInternal,
18948 cartesian_trajectory_planner_B.b_kstr_c);
18949 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18950 cartesian_trajectory_planner_B.b_kstr_c < 2;
18951 cartesian_trajectory_planner_B.b_kstr_c++) {
18952 iobj_15->PositionLimitsInternal->
18953 data[cartesian_trajectory_planner_B.b_kstr_c] =
18954 cartesian_trajectory_planner_B.poslim_data[cartesian_trajectory_planner_B.b_kstr_c];
18955 }
18956
18957 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->HomePositionInternal->size
18958 [0];
18959 iobj_15->HomePositionInternal->size[0] = 1;
18960 cartes_emxEnsureCapacity_real_T(iobj_15->HomePositionInternal,
18961 cartesian_trajectory_planner_B.b_kstr_c);
18962 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
18963 cartesian_trajectory_planner_B.b_kstr_c < 1;
18964 cartesian_trajectory_planner_B.b_kstr_c++) {
18965 iobj_15->HomePositionInternal->data[0] = 0.0;
18966 }
18967
18968 obj->Base.JointInternal = iobj_15;
18969 obj->Base.Index = -1.0;
18970 obj->Base.ParentIndex = -1.0;
18971 obj->Base.Index = 0.0;
18972 cartesian_trajectory_plann_rand(cartesian_trajectory_planner_B.unusedExpr);
18973 ca_RigidBodyTree_clearAllBodies(obj, iobj_0, iobj_1, iobj_2, iobj_3, iobj_4,
18974 iobj_5, iobj_6, iobj_8, iobj_9, iobj_10, iobj_11, iobj_12, iobj_13, iobj_14,
18975 iobj_7, iobj_16);
18976 return b_obj;
18977}
18978
18979static c_rigidBodyJoint_cartesian_as_T *c_rigidBodyJoint_rigidBodyJoint
18980 (c_rigidBodyJoint_cartesian_as_T *obj, const emxArray_char_T_cartesian_tra_T
18981 *jname, const emxArray_char_T_cartesian_tra_T *jtype)
18982{
18983 c_rigidBodyJoint_cartesian_as_T *b_obj;
18984 emxArray_char_T_cartesian_tra_T *switch_expression;
18985 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
18986 };
18987
18988 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18989
18990 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18991
18992 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
18993
18994 static const char_T tmp_3[128] = { '\x00', '\x01', '\x02', '\x03', '\x04',
18995 '\x05', '\x06', '\x07', '\x08', '\x09', '\x0a', '\x0b', '\x0c', '\x0d',
18996 '\x0e', '\x0f', '\x10', '\x11', '\x12', '\x13', '\x14', '\x15', '\x16',
18997 '\x17', '\x18', '\x19', '\x1a', '\x1b', '\x1c', '\x1d', '\x1e', '\x1f', ' ',
18998 '!', '\"', '#', '$', '%', '&', '\'', '(', ')', '*', '+', ',', '-', '.', '/',
18999 '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', ':', ';', '<', '=', '>',
19000 '?', '@', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm',
19001 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', '[', '\\',
19002 ']', '^', '_', '`', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k',
19003 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z',
19004 '{', '|', '}', '~', '\x7f' };
19005
19006 boolean_T guard1 = false;
19007 boolean_T guard2 = false;
19008 boolean_T guard3 = false;
19009 boolean_T guard4 = false;
19010 int32_T exitg1;
19011 boolean_T guard11 = false;
19012 obj->InTree = false;
19013 for (cartesian_trajectory_planner_B.minnanb = 0;
19014 cartesian_trajectory_planner_B.minnanb < 16;
19015 cartesian_trajectory_planner_B.minnanb++) {
19016 obj->JointToParentTransform[cartesian_trajectory_planner_B.minnanb] =
19017 tmp[cartesian_trajectory_planner_B.minnanb];
19018 }
19019
19020 for (cartesian_trajectory_planner_B.minnanb = 0;
19021 cartesian_trajectory_planner_B.minnanb < 16;
19022 cartesian_trajectory_planner_B.minnanb++) {
19023 obj->ChildToJointTransform[cartesian_trajectory_planner_B.minnanb] =
19024 tmp[cartesian_trajectory_planner_B.minnanb];
19025 }
19026
19027 b_obj = obj;
19028 cartesian_trajectory_planner_B.minnanb = obj->NameInternal->size[0] *
19029 obj->NameInternal->size[1];
19030 obj->NameInternal->size[0] = 1;
19031 obj->NameInternal->size[1] = jname->size[1];
19032 cart_emxEnsureCapacity_char_T_a(obj->NameInternal,
19033 cartesian_trajectory_planner_B.minnanb);
19034 cartesian_trajectory_planner_B.loop_ub_hb = jname->size[0] * jname->size[1] -
19035 1;
19036 for (cartesian_trajectory_planner_B.minnanb = 0;
19037 cartesian_trajectory_planner_B.minnanb <=
19038 cartesian_trajectory_planner_B.loop_ub_hb;
19039 cartesian_trajectory_planner_B.minnanb++) {
19040 obj->NameInternal->data[cartesian_trajectory_planner_B.minnanb] =
19041 jname->data[cartesian_trajectory_planner_B.minnanb];
19042 }
19043
19044 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 0;
19045 for (cartesian_trajectory_planner_B.minnanb = 0;
19046 cartesian_trajectory_planner_B.minnanb < 8;
19047 cartesian_trajectory_planner_B.minnanb++) {
19048 cartesian_trajectory_planner_B.vstr[cartesian_trajectory_planner_B.minnanb] =
19049 tmp_0[cartesian_trajectory_planner_B.minnanb];
19050 }
19051
19052 guard1 = false;
19053 guard2 = false;
19054 guard3 = false;
19055 guard4 = false;
19056 if (jtype->size[1] <= 8) {
19057 cartesian_trajectory_planner_B.loop_ub_hb = jtype->size[1];
19058 for (cartesian_trajectory_planner_B.minnanb = 0;
19059 cartesian_trajectory_planner_B.minnanb < 8;
19060 cartesian_trajectory_planner_B.minnanb++) {
19061 cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.minnanb]
19062 = tmp_0[cartesian_trajectory_planner_B.minnanb];
19063 }
19064
19065 cartesian_trajectory_planner_B.b_bool_nh = false;
19066 cartesian_trajectory_planner_B.minnanb = jtype->size[1];
19067 if (cartesian_trajectory_planner_B.minnanb >= 8) {
19068 cartesian_trajectory_planner_B.minnanb = 8;
19069 }
19070
19071 guard11 = false;
19072 if (cartesian_trajectory_planner_B.loop_ub_hb <=
19073 cartesian_trajectory_planner_B.minnanb) {
19074 if (cartesian_trajectory_planner_B.minnanb <
19075 cartesian_trajectory_planner_B.loop_ub_hb) {
19076 cartesian_trajectory_planner_B.loop_ub_hb =
19077 cartesian_trajectory_planner_B.minnanb;
19078 }
19079
19080 cartesian_trajectory_planner_B.minnanb =
19081 cartesian_trajectory_planner_B.loop_ub_hb - 1;
19082 guard11 = true;
19083 } else {
19084 if (jtype->size[1] == 8) {
19085 cartesian_trajectory_planner_B.minnanb = 7;
19086 guard11 = true;
19087 }
19088 }
19089
19090 if (guard11) {
19091 cartesian_trajectory_planner_B.loop_ub_hb = 1;
19092 do {
19093 exitg1 = 0;
19094 if (cartesian_trajectory_planner_B.loop_ub_hb - 1 <=
19095 cartesian_trajectory_planner_B.minnanb) {
19096 cartesian_trajectory_planner_B.kstr_l =
19097 cartesian_trajectory_planner_B.loop_ub_hb - 1;
19098 if (tmp_3[static_cast<uint8_T>(jtype->
19099 data[cartesian_trajectory_planner_B.kstr_l]) & 127] != tmp_3[
19100 static_cast<int32_T>
19101 (cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.kstr_l])])
19102 {
19103 exitg1 = 1;
19104 } else {
19105 cartesian_trajectory_planner_B.loop_ub_hb++;
19106 }
19107 } else {
19108 cartesian_trajectory_planner_B.b_bool_nh = true;
19109 exitg1 = 1;
19110 }
19111 } while (exitg1 == 0);
19112 }
19113
19114 if (cartesian_trajectory_planner_B.b_bool_nh) {
19115 if (jtype->size[1] == 8) {
19116 cartesian_trajectory_planner_B.nmatched = 1;
19117 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 8;
19118 for (cartesian_trajectory_planner_B.minnanb = 0;
19119 cartesian_trajectory_planner_B.minnanb < 8;
19120 cartesian_trajectory_planner_B.minnanb++) {
19121 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
19122 =
19123 cartesian_trajectory_planner_B.vstr[cartesian_trajectory_planner_B.minnanb];
19124 }
19125 } else {
19126 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 8;
19127 for (cartesian_trajectory_planner_B.minnanb = 0;
19128 cartesian_trajectory_planner_B.minnanb < 8;
19129 cartesian_trajectory_planner_B.minnanb++) {
19130 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb]
19131 =
19132 cartesian_trajectory_planner_B.vstr[cartesian_trajectory_planner_B.minnanb];
19133 }
19134
19135 cartesian_trajectory_planner_B.matched = true;
19136 cartesian_trajectory_planner_B.nmatched = 1;
19137 guard3 = true;
19138 }
19139 } else {
19140 guard4 = true;
19141 }
19142 } else {
19143 guard4 = true;
19144 }
19145
19146 if (guard4) {
19147 cartesian_trajectory_planner_B.matched = false;
19148 cartesian_trajectory_planner_B.nmatched = 0;
19149 guard3 = true;
19150 }
19151
19152 if (guard3) {
19153 for (cartesian_trajectory_planner_B.minnanb = 0;
19154 cartesian_trajectory_planner_B.minnanb < 9;
19155 cartesian_trajectory_planner_B.minnanb++) {
19156 cartesian_trajectory_planner_B.b_vstr[cartesian_trajectory_planner_B.minnanb]
19157 = tmp_1[cartesian_trajectory_planner_B.minnanb];
19158 }
19159
19160 if (jtype->size[1] <= 9) {
19161 cartesian_trajectory_planner_B.loop_ub_hb = jtype->size[1];
19162 for (cartesian_trajectory_planner_B.minnanb = 0;
19163 cartesian_trajectory_planner_B.minnanb < 9;
19164 cartesian_trajectory_planner_B.minnanb++) {
19165 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
19166 = tmp_1[cartesian_trajectory_planner_B.minnanb];
19167 }
19168
19169 cartesian_trajectory_planner_B.b_bool_nh = false;
19170 cartesian_trajectory_planner_B.minnanb = jtype->size[1];
19171 if (cartesian_trajectory_planner_B.minnanb >= 9) {
19172 cartesian_trajectory_planner_B.minnanb = 9;
19173 }
19174
19175 guard11 = false;
19176 if (cartesian_trajectory_planner_B.loop_ub_hb <=
19177 cartesian_trajectory_planner_B.minnanb) {
19178 if (cartesian_trajectory_planner_B.minnanb <
19179 cartesian_trajectory_planner_B.loop_ub_hb) {
19180 cartesian_trajectory_planner_B.loop_ub_hb =
19181 cartesian_trajectory_planner_B.minnanb;
19182 }
19183
19184 cartesian_trajectory_planner_B.minnanb =
19185 cartesian_trajectory_planner_B.loop_ub_hb - 1;
19186 guard11 = true;
19187 } else {
19188 if (jtype->size[1] == 9) {
19189 cartesian_trajectory_planner_B.minnanb = 8;
19190 guard11 = true;
19191 }
19192 }
19193
19194 if (guard11) {
19195 cartesian_trajectory_planner_B.loop_ub_hb = 1;
19196 do {
19197 exitg1 = 0;
19198 if (cartesian_trajectory_planner_B.loop_ub_hb - 1 <=
19199 cartesian_trajectory_planner_B.minnanb) {
19200 cartesian_trajectory_planner_B.kstr_l =
19201 cartesian_trajectory_planner_B.loop_ub_hb - 1;
19202 if (tmp_3[static_cast<uint8_T>(jtype->
19203 data[cartesian_trajectory_planner_B.kstr_l]) & 127] != tmp_3[
19204 static_cast<int32_T>
19205 (cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.kstr_l])])
19206 {
19207 exitg1 = 1;
19208 } else {
19209 cartesian_trajectory_planner_B.loop_ub_hb++;
19210 }
19211 } else {
19212 cartesian_trajectory_planner_B.b_bool_nh = true;
19213 exitg1 = 1;
19214 }
19215 } while (exitg1 == 0);
19216 }
19217
19218 if (cartesian_trajectory_planner_B.b_bool_nh) {
19219 if (jtype->size[1] == 9) {
19220 cartesian_trajectory_planner_B.nmatched = 1;
19221 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 9;
19222 for (cartesian_trajectory_planner_B.minnanb = 0;
19223 cartesian_trajectory_planner_B.minnanb < 9;
19224 cartesian_trajectory_planner_B.minnanb++) {
19225 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
19226 =
19227 cartesian_trajectory_planner_B.b_vstr[cartesian_trajectory_planner_B.minnanb];
19228 }
19229 } else {
19230 if (!cartesian_trajectory_planner_B.matched) {
19231 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 9;
19232 for (cartesian_trajectory_planner_B.minnanb = 0;
19233 cartesian_trajectory_planner_B.minnanb < 9;
19234 cartesian_trajectory_planner_B.minnanb++) {
19235 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb]
19236 =
19237 cartesian_trajectory_planner_B.b_vstr[cartesian_trajectory_planner_B.minnanb];
19238 }
19239 }
19240
19241 cartesian_trajectory_planner_B.matched = true;
19242 cartesian_trajectory_planner_B.nmatched++;
19243 guard2 = true;
19244 }
19245 } else {
19246 guard2 = true;
19247 }
19248 } else {
19249 guard2 = true;
19250 }
19251 }
19252
19253 if (guard2) {
19254 for (cartesian_trajectory_planner_B.minnanb = 0;
19255 cartesian_trajectory_planner_B.minnanb < 5;
19256 cartesian_trajectory_planner_B.minnanb++) {
19257 cartesian_trajectory_planner_B.c_vstr[cartesian_trajectory_planner_B.minnanb]
19258 = tmp_2[cartesian_trajectory_planner_B.minnanb];
19259 }
19260
19261 if (jtype->size[1] <= 5) {
19262 cartesian_trajectory_planner_B.loop_ub_hb = jtype->size[1];
19263 for (cartesian_trajectory_planner_B.minnanb = 0;
19264 cartesian_trajectory_planner_B.minnanb < 5;
19265 cartesian_trajectory_planner_B.minnanb++) {
19266 cartesian_trajectory_planner_B.b_js[cartesian_trajectory_planner_B.minnanb]
19267 = tmp_2[cartesian_trajectory_planner_B.minnanb];
19268 }
19269
19270 cartesian_trajectory_planner_B.b_bool_nh = false;
19271 cartesian_trajectory_planner_B.minnanb = jtype->size[1];
19272 if (cartesian_trajectory_planner_B.minnanb >= 5) {
19273 cartesian_trajectory_planner_B.minnanb = 5;
19274 }
19275
19276 guard11 = false;
19277 if (cartesian_trajectory_planner_B.loop_ub_hb <=
19278 cartesian_trajectory_planner_B.minnanb) {
19279 if (cartesian_trajectory_planner_B.minnanb <
19280 cartesian_trajectory_planner_B.loop_ub_hb) {
19281 cartesian_trajectory_planner_B.loop_ub_hb =
19282 cartesian_trajectory_planner_B.minnanb;
19283 }
19284
19285 cartesian_trajectory_planner_B.minnanb =
19286 cartesian_trajectory_planner_B.loop_ub_hb - 1;
19287 guard11 = true;
19288 } else {
19289 if (jtype->size[1] == 5) {
19290 cartesian_trajectory_planner_B.minnanb = 4;
19291 guard11 = true;
19292 }
19293 }
19294
19295 if (guard11) {
19296 cartesian_trajectory_planner_B.loop_ub_hb = 1;
19297 do {
19298 exitg1 = 0;
19299 if (cartesian_trajectory_planner_B.loop_ub_hb - 1 <=
19300 cartesian_trajectory_planner_B.minnanb) {
19301 cartesian_trajectory_planner_B.kstr_l =
19302 cartesian_trajectory_planner_B.loop_ub_hb - 1;
19303 if (tmp_3[static_cast<uint8_T>(jtype->
19304 data[cartesian_trajectory_planner_B.kstr_l]) & 127] != tmp_3[
19305 static_cast<int32_T>
19306 (cartesian_trajectory_planner_B.b_js[cartesian_trajectory_planner_B.kstr_l])])
19307 {
19308 exitg1 = 1;
19309 } else {
19310 cartesian_trajectory_planner_B.loop_ub_hb++;
19311 }
19312 } else {
19313 cartesian_trajectory_planner_B.b_bool_nh = true;
19314 exitg1 = 1;
19315 }
19316 } while (exitg1 == 0);
19317 }
19318
19319 if (cartesian_trajectory_planner_B.b_bool_nh) {
19320 if (jtype->size[1] == 5) {
19321 cartesian_trajectory_planner_B.nmatched = 1;
19322 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 5;
19323 for (cartesian_trajectory_planner_B.minnanb = 0;
19324 cartesian_trajectory_planner_B.minnanb < 5;
19325 cartesian_trajectory_planner_B.minnanb++) {
19326 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
19327 =
19328 cartesian_trajectory_planner_B.c_vstr[cartesian_trajectory_planner_B.minnanb];
19329 }
19330 } else {
19331 if (!cartesian_trajectory_planner_B.matched) {
19332 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 5;
19333 for (cartesian_trajectory_planner_B.minnanb = 0;
19334 cartesian_trajectory_planner_B.minnanb < 5;
19335 cartesian_trajectory_planner_B.minnanb++) {
19336 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb]
19337 =
19338 cartesian_trajectory_planner_B.c_vstr[cartesian_trajectory_planner_B.minnanb];
19339 }
19340 }
19341
19342 cartesian_trajectory_planner_B.nmatched++;
19343 guard1 = true;
19344 }
19345 } else {
19346 guard1 = true;
19347 }
19348 } else {
19349 guard1 = true;
19350 }
19351 }
19352
19353 if (guard1) {
19354 if (cartesian_trajectory_planner_B.nmatched == 0) {
19355 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 0;
19356 } else {
19357 cartesian_trajectory_planner_B.loop_ub_hb =
19358 cartesian_trajectory_planner_B.partial_match_size_idx_1 - 1;
19359 if (0 <= cartesian_trajectory_planner_B.loop_ub_hb) {
19360 memcpy(&cartesian_trajectory_planner_B.b_na[0],
19361 &cartesian_trajectory_planner_B.partial_match_data[0],
19362 (cartesian_trajectory_planner_B.loop_ub_hb + 1) * sizeof(char_T));
19363 }
19364 }
19365 }
19366
19367 if ((cartesian_trajectory_planner_B.nmatched == 0) || ((jtype->size[1] == 0)
19368 != (cartesian_trajectory_planner_B.partial_match_size_idx_1 == 0))) {
19369 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 0;
19370 } else {
19371 cartesian_trajectory_planner_B.loop_ub_hb =
19372 cartesian_trajectory_planner_B.partial_match_size_idx_1 - 1;
19373 if (0 <= cartesian_trajectory_planner_B.loop_ub_hb) {
19374 memcpy(&cartesian_trajectory_planner_B.partial_match_data[0],
19375 &cartesian_trajectory_planner_B.b_na[0],
19376 (cartesian_trajectory_planner_B.loop_ub_hb + 1) * sizeof(char_T));
19377 }
19378 }
19379
19380 cartesian_trajectory_planner_B.minnanb = obj->Type->size[0] * obj->Type->size
19381 [1];
19382 obj->Type->size[0] = 1;
19383 obj->Type->size[1] = cartesian_trajectory_planner_B.partial_match_size_idx_1;
19384 cart_emxEnsureCapacity_char_T_a(obj->Type,
19385 cartesian_trajectory_planner_B.minnanb);
19386 cartesian_trajectory_planner_B.loop_ub_hb =
19387 cartesian_trajectory_planner_B.partial_match_size_idx_1 - 1;
19388 for (cartesian_trajectory_planner_B.minnanb = 0;
19389 cartesian_trajectory_planner_B.minnanb <=
19390 cartesian_trajectory_planner_B.loop_ub_hb;
19391 cartesian_trajectory_planner_B.minnanb++) {
19392 obj->Type->data[cartesian_trajectory_planner_B.minnanb] =
19393 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb];
19394 }
19395
19396 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
19397 cartesian_trajectory_planner_B.minnanb = switch_expression->size[0] *
19398 switch_expression->size[1];
19399 switch_expression->size[0] = 1;
19400 switch_expression->size[1] = obj->Type->size[1];
19401 cart_emxEnsureCapacity_char_T_a(switch_expression,
19402 cartesian_trajectory_planner_B.minnanb);
19403 cartesian_trajectory_planner_B.loop_ub_hb = obj->Type->size[0] * obj->
19404 Type->size[1] - 1;
19405 for (cartesian_trajectory_planner_B.minnanb = 0;
19406 cartesian_trajectory_planner_B.minnanb <=
19407 cartesian_trajectory_planner_B.loop_ub_hb;
19408 cartesian_trajectory_planner_B.minnanb++) {
19409 switch_expression->data[cartesian_trajectory_planner_B.minnanb] = obj->
19410 Type->data[cartesian_trajectory_planner_B.minnanb];
19411 }
19412
19413 for (cartesian_trajectory_planner_B.minnanb = 0;
19414 cartesian_trajectory_planner_B.minnanb < 8;
19415 cartesian_trajectory_planner_B.minnanb++) {
19416 cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.minnanb] =
19417 tmp_0[cartesian_trajectory_planner_B.minnanb];
19418 }
19419
19420 cartesian_trajectory_planner_B.b_bool_nh = false;
19421 if (switch_expression->size[1] == 8) {
19422 cartesian_trajectory_planner_B.loop_ub_hb = 1;
19423 do {
19424 exitg1 = 0;
19425 if (cartesian_trajectory_planner_B.loop_ub_hb - 1 < 8) {
19426 cartesian_trajectory_planner_B.kstr_l =
19427 cartesian_trajectory_planner_B.loop_ub_hb - 1;
19428 if (switch_expression->data[cartesian_trajectory_planner_B.kstr_l] !=
19429 cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.kstr_l])
19430 {
19431 exitg1 = 1;
19432 } else {
19433 cartesian_trajectory_planner_B.loop_ub_hb++;
19434 }
19435 } else {
19436 cartesian_trajectory_planner_B.b_bool_nh = true;
19437 exitg1 = 1;
19438 }
19439 } while (exitg1 == 0);
19440 }
19441
19442 if (cartesian_trajectory_planner_B.b_bool_nh) {
19443 cartesian_trajectory_planner_B.minnanb = 0;
19444 } else {
19445 for (cartesian_trajectory_planner_B.minnanb = 0;
19446 cartesian_trajectory_planner_B.minnanb < 9;
19447 cartesian_trajectory_planner_B.minnanb++) {
19448 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
19449 = tmp_1[cartesian_trajectory_planner_B.minnanb];
19450 }
19451
19452 cartesian_trajectory_planner_B.b_bool_nh = false;
19453 if (switch_expression->size[1] == 9) {
19454 cartesian_trajectory_planner_B.loop_ub_hb = 1;
19455 do {
19456 exitg1 = 0;
19457 if (cartesian_trajectory_planner_B.loop_ub_hb - 1 < 9) {
19458 cartesian_trajectory_planner_B.kstr_l =
19459 cartesian_trajectory_planner_B.loop_ub_hb - 1;
19460 if (switch_expression->data[cartesian_trajectory_planner_B.kstr_l] !=
19461 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.kstr_l])
19462 {
19463 exitg1 = 1;
19464 } else {
19465 cartesian_trajectory_planner_B.loop_ub_hb++;
19466 }
19467 } else {
19468 cartesian_trajectory_planner_B.b_bool_nh = true;
19469 exitg1 = 1;
19470 }
19471 } while (exitg1 == 0);
19472 }
19473
19474 if (cartesian_trajectory_planner_B.b_bool_nh) {
19475 cartesian_trajectory_planner_B.minnanb = 1;
19476 } else {
19477 cartesian_trajectory_planner_B.minnanb = -1;
19478 }
19479 }
19480
19481 cartesian_traj_emxFree_char_T_a(&switch_expression);
19482 switch (cartesian_trajectory_planner_B.minnanb) {
19483 case 0:
19484 cartesian_trajectory_planner_B.iv3[0] = 0;
19485 cartesian_trajectory_planner_B.iv3[1] = 0;
19486 cartesian_trajectory_planner_B.iv3[2] = 1;
19487 cartesian_trajectory_planner_B.iv3[3] = 0;
19488 cartesian_trajectory_planner_B.iv3[4] = 0;
19489 cartesian_trajectory_planner_B.iv3[5] = 0;
19490 for (cartesian_trajectory_planner_B.minnanb = 0;
19491 cartesian_trajectory_planner_B.minnanb < 6;
19492 cartesian_trajectory_planner_B.minnanb++) {
19493 cartesian_trajectory_planner_B.msubspace_data_o[cartesian_trajectory_planner_B.minnanb]
19494 =
19495 cartesian_trajectory_planner_B.iv3[cartesian_trajectory_planner_B.minnanb];
19496 }
19497
19498 cartesian_trajectory_planner_B.poslim_data_i[0] = -3.1415926535897931;
19499 cartesian_trajectory_planner_B.poslim_data_i[1] = 3.1415926535897931;
19500 obj->VelocityNumber = 1.0;
19501 obj->PositionNumber = 1.0;
19502 obj->JointAxisInternal[0] = 0.0;
19503 obj->JointAxisInternal[1] = 0.0;
19504 obj->JointAxisInternal[2] = 1.0;
19505 break;
19506
19507 case 1:
19508 cartesian_trajectory_planner_B.iv3[0] = 0;
19509 cartesian_trajectory_planner_B.iv3[1] = 0;
19510 cartesian_trajectory_planner_B.iv3[2] = 0;
19511 cartesian_trajectory_planner_B.iv3[3] = 0;
19512 cartesian_trajectory_planner_B.iv3[4] = 0;
19513 cartesian_trajectory_planner_B.iv3[5] = 1;
19514 for (cartesian_trajectory_planner_B.minnanb = 0;
19515 cartesian_trajectory_planner_B.minnanb < 6;
19516 cartesian_trajectory_planner_B.minnanb++) {
19517 cartesian_trajectory_planner_B.msubspace_data_o[cartesian_trajectory_planner_B.minnanb]
19518 =
19519 cartesian_trajectory_planner_B.iv3[cartesian_trajectory_planner_B.minnanb];
19520 }
19521
19522 cartesian_trajectory_planner_B.poslim_data_i[0] = -0.5;
19523 cartesian_trajectory_planner_B.poslim_data_i[1] = 0.5;
19524 obj->VelocityNumber = 1.0;
19525 obj->PositionNumber = 1.0;
19526 obj->JointAxisInternal[0] = 0.0;
19527 obj->JointAxisInternal[1] = 0.0;
19528 obj->JointAxisInternal[2] = 1.0;
19529 break;
19530
19531 default:
19532 for (cartesian_trajectory_planner_B.minnanb = 0;
19533 cartesian_trajectory_planner_B.minnanb < 6;
19534 cartesian_trajectory_planner_B.minnanb++) {
19535 cartesian_trajectory_planner_B.msubspace_data_o[cartesian_trajectory_planner_B.minnanb]
19536 = 0;
19537 }
19538
19539 cartesian_trajectory_planner_B.poslim_data_i[0] = 0.0;
19540 cartesian_trajectory_planner_B.poslim_data_i[1] = 0.0;
19541 obj->VelocityNumber = 0.0;
19542 obj->PositionNumber = 0.0;
19543 obj->JointAxisInternal[0] = 0.0;
19544 obj->JointAxisInternal[1] = 0.0;
19545 obj->JointAxisInternal[2] = 0.0;
19546 break;
19547 }
19548
19549 cartesian_trajectory_planner_B.minnanb = obj->MotionSubspace->size[0] *
19550 obj->MotionSubspace->size[1];
19551 obj->MotionSubspace->size[0] = 6;
19552 obj->MotionSubspace->size[1] = 1;
19553 cartes_emxEnsureCapacity_real_T(obj->MotionSubspace,
19554 cartesian_trajectory_planner_B.minnanb);
19555 for (cartesian_trajectory_planner_B.minnanb = 0;
19556 cartesian_trajectory_planner_B.minnanb < 6;
19557 cartesian_trajectory_planner_B.minnanb++) {
19558 obj->MotionSubspace->data[cartesian_trajectory_planner_B.minnanb] =
19559 cartesian_trajectory_planner_B.msubspace_data_o[cartesian_trajectory_planner_B.minnanb];
19560 }
19561
19562 cartesian_trajectory_planner_B.minnanb = obj->PositionLimitsInternal->size[0] *
19563 obj->PositionLimitsInternal->size[1];
19564 obj->PositionLimitsInternal->size[0] = 1;
19565 obj->PositionLimitsInternal->size[1] = 2;
19566 cartes_emxEnsureCapacity_real_T(obj->PositionLimitsInternal,
19567 cartesian_trajectory_planner_B.minnanb);
19568 for (cartesian_trajectory_planner_B.minnanb = 0;
19569 cartesian_trajectory_planner_B.minnanb < 2;
19570 cartesian_trajectory_planner_B.minnanb++) {
19571 obj->PositionLimitsInternal->data[cartesian_trajectory_planner_B.minnanb] =
19572 cartesian_trajectory_planner_B.poslim_data_i[cartesian_trajectory_planner_B.minnanb];
19573 }
19574
19575 cartesian_trajectory_planner_B.minnanb = obj->HomePositionInternal->size[0];
19576 obj->HomePositionInternal->size[0] = 1;
19577 cartes_emxEnsureCapacity_real_T(obj->HomePositionInternal,
19578 cartesian_trajectory_planner_B.minnanb);
19579 for (cartesian_trajectory_planner_B.minnanb = 0;
19580 cartesian_trajectory_planner_B.minnanb < 1;
19581 cartesian_trajectory_planner_B.minnanb++) {
19582 obj->HomePositionInternal->data[0] = 0.0;
19583 }
19584
19585 return b_obj;
19586}
19587
19588static w_robotics_manip_internal_Rig_T *cartesian_trajec_RigidBody_copy(const
19589 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_as_T *iobj_0,
19590 c_rigidBodyJoint_cartesian_as_T *iobj_1, w_robotics_manip_internal_Rig_T
19591 *iobj_2)
19592{
19593 w_robotics_manip_internal_Rig_T *newbody;
19594 c_rigidBodyJoint_cartesian_as_T *newjoint;
19595 emxArray_char_T_cartesian_tra_T *jtype;
19596 emxArray_char_T_cartesian_tra_T *jname;
19597 emxArray_real_T_cartesian_tra_T *obj_0;
19598 emxArray_real_T_cartesian_tra_T *obj_1;
19599 emxArray_real_T_cartesian_tra_T *obj_2;
19600 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
19601 };
19602
19603 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
19604
19605 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19606
19607 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19608
19609 int32_T exitg1;
19610 cartesian_traj_emxInit_char_T_a(&jtype, 2);
19611 cartesian_trajectory_planner_B.b_kstr_po = jtype->size[0] * jtype->size[1];
19612 jtype->size[0] = 1;
19613 jtype->size[1] = obj->NameInternal->size[1];
19614 cart_emxEnsureCapacity_char_T_a(jtype,
19615 cartesian_trajectory_planner_B.b_kstr_po);
19616 cartesian_trajectory_planner_B.loop_ub_ly = obj->NameInternal->size[0] *
19617 obj->NameInternal->size[1] - 1;
19618 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19619 cartesian_trajectory_planner_B.b_kstr_po <=
19620 cartesian_trajectory_planner_B.loop_ub_ly;
19621 cartesian_trajectory_planner_B.b_kstr_po++) {
19622 jtype->data[cartesian_trajectory_planner_B.b_kstr_po] = obj->
19623 NameInternal->data[cartesian_trajectory_planner_B.b_kstr_po];
19624 }
19625
19626 newbody = iobj_2;
19627 cartesian_trajectory_planner_B.b_kstr_po = iobj_2->NameInternal->size[0] *
19628 iobj_2->NameInternal->size[1];
19629 iobj_2->NameInternal->size[0] = 1;
19630 iobj_2->NameInternal->size[1] = jtype->size[1];
19631 cart_emxEnsureCapacity_char_T_a(iobj_2->NameInternal,
19632 cartesian_trajectory_planner_B.b_kstr_po);
19633 cartesian_trajectory_planner_B.loop_ub_ly = jtype->size[0] * jtype->size[1] -
19634 1;
19635 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19636 cartesian_trajectory_planner_B.b_kstr_po <=
19637 cartesian_trajectory_planner_B.loop_ub_ly;
19638 cartesian_trajectory_planner_B.b_kstr_po++) {
19639 iobj_2->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_po] =
19640 jtype->data[cartesian_trajectory_planner_B.b_kstr_po];
19641 }
19642
19643 iobj_0->InTree = false;
19644 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19645 cartesian_trajectory_planner_B.b_kstr_po < 16;
19646 cartesian_trajectory_planner_B.b_kstr_po++) {
19647 iobj_0->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_po] =
19648 tmp[cartesian_trajectory_planner_B.b_kstr_po];
19649 }
19650
19651 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19652 cartesian_trajectory_planner_B.b_kstr_po < 16;
19653 cartesian_trajectory_planner_B.b_kstr_po++) {
19654 iobj_0->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_po] =
19655 tmp[cartesian_trajectory_planner_B.b_kstr_po];
19656 }
19657
19658 cartesian_trajectory_planner_B.b_kstr_po = iobj_0->NameInternal->size[0] *
19659 iobj_0->NameInternal->size[1];
19660 iobj_0->NameInternal->size[0] = 1;
19661 iobj_0->NameInternal->size[1] = jtype->size[1] + 4;
19662 cart_emxEnsureCapacity_char_T_a(iobj_0->NameInternal,
19663 cartesian_trajectory_planner_B.b_kstr_po);
19664 cartesian_trajectory_planner_B.loop_ub_ly = jtype->size[0] * jtype->size[1];
19665 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19666 cartesian_trajectory_planner_B.b_kstr_po <
19667 cartesian_trajectory_planner_B.loop_ub_ly;
19668 cartesian_trajectory_planner_B.b_kstr_po++) {
19669 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_po] =
19670 jtype->data[cartesian_trajectory_planner_B.b_kstr_po];
19671 }
19672
19673 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_ly] = '_';
19674 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_ly + 1] =
19675 'j';
19676 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_ly + 2] =
19677 'n';
19678 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_ly + 3] =
19679 't';
19680 cartesian_trajectory_planner_B.b_kstr_po = iobj_0->Type->size[0] *
19681 iobj_0->Type->size[1];
19682 iobj_0->Type->size[0] = 1;
19683 iobj_0->Type->size[1] = 5;
19684 cart_emxEnsureCapacity_char_T_a(iobj_0->Type,
19685 cartesian_trajectory_planner_B.b_kstr_po);
19686 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19687 cartesian_trajectory_planner_B.b_kstr_po < 5;
19688 cartesian_trajectory_planner_B.b_kstr_po++) {
19689 iobj_0->Type->data[cartesian_trajectory_planner_B.b_kstr_po] =
19690 tmp_0[cartesian_trajectory_planner_B.b_kstr_po];
19691 }
19692
19693 cartesian_trajectory_planner_B.b_kstr_po = jtype->size[0] * jtype->size[1];
19694 jtype->size[0] = 1;
19695 jtype->size[1] = iobj_0->Type->size[1];
19696 cart_emxEnsureCapacity_char_T_a(jtype,
19697 cartesian_trajectory_planner_B.b_kstr_po);
19698 cartesian_trajectory_planner_B.loop_ub_ly = iobj_0->Type->size[0] *
19699 iobj_0->Type->size[1] - 1;
19700 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19701 cartesian_trajectory_planner_B.b_kstr_po <=
19702 cartesian_trajectory_planner_B.loop_ub_ly;
19703 cartesian_trajectory_planner_B.b_kstr_po++) {
19704 jtype->data[cartesian_trajectory_planner_B.b_kstr_po] = iobj_0->Type->
19705 data[cartesian_trajectory_planner_B.b_kstr_po];
19706 }
19707
19708 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19709 cartesian_trajectory_planner_B.b_kstr_po < 8;
19710 cartesian_trajectory_planner_B.b_kstr_po++) {
19711 cartesian_trajectory_planner_B.b_gg[cartesian_trajectory_planner_B.b_kstr_po]
19712 = tmp_1[cartesian_trajectory_planner_B.b_kstr_po];
19713 }
19714
19715 cartesian_trajectory_planner_B.b_bool_ce = false;
19716 if (jtype->size[1] == 8) {
19717 cartesian_trajectory_planner_B.b_kstr_po = 1;
19718 do {
19719 exitg1 = 0;
19720 if (cartesian_trajectory_planner_B.b_kstr_po - 1 < 8) {
19721 cartesian_trajectory_planner_B.loop_ub_ly =
19722 cartesian_trajectory_planner_B.b_kstr_po - 1;
19723 if (jtype->data[cartesian_trajectory_planner_B.loop_ub_ly] !=
19724 cartesian_trajectory_planner_B.b_gg[cartesian_trajectory_planner_B.loop_ub_ly])
19725 {
19726 exitg1 = 1;
19727 } else {
19728 cartesian_trajectory_planner_B.b_kstr_po++;
19729 }
19730 } else {
19731 cartesian_trajectory_planner_B.b_bool_ce = true;
19732 exitg1 = 1;
19733 }
19734 } while (exitg1 == 0);
19735 }
19736
19737 if (cartesian_trajectory_planner_B.b_bool_ce) {
19738 cartesian_trajectory_planner_B.b_kstr_po = 0;
19739 } else {
19740 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19741 cartesian_trajectory_planner_B.b_kstr_po < 9;
19742 cartesian_trajectory_planner_B.b_kstr_po++) {
19743 cartesian_trajectory_planner_B.b_d[cartesian_trajectory_planner_B.b_kstr_po]
19744 = tmp_2[cartesian_trajectory_planner_B.b_kstr_po];
19745 }
19746
19747 cartesian_trajectory_planner_B.b_bool_ce = false;
19748 if (jtype->size[1] == 9) {
19749 cartesian_trajectory_planner_B.b_kstr_po = 1;
19750 do {
19751 exitg1 = 0;
19752 if (cartesian_trajectory_planner_B.b_kstr_po - 1 < 9) {
19753 cartesian_trajectory_planner_B.loop_ub_ly =
19754 cartesian_trajectory_planner_B.b_kstr_po - 1;
19755 if (jtype->data[cartesian_trajectory_planner_B.loop_ub_ly] !=
19756 cartesian_trajectory_planner_B.b_d[cartesian_trajectory_planner_B.loop_ub_ly])
19757 {
19758 exitg1 = 1;
19759 } else {
19760 cartesian_trajectory_planner_B.b_kstr_po++;
19761 }
19762 } else {
19763 cartesian_trajectory_planner_B.b_bool_ce = true;
19764 exitg1 = 1;
19765 }
19766 } while (exitg1 == 0);
19767 }
19768
19769 if (cartesian_trajectory_planner_B.b_bool_ce) {
19770 cartesian_trajectory_planner_B.b_kstr_po = 1;
19771 } else {
19772 cartesian_trajectory_planner_B.b_kstr_po = -1;
19773 }
19774 }
19775
19776 switch (cartesian_trajectory_planner_B.b_kstr_po) {
19777 case 0:
19778 cartesian_trajectory_planner_B.iv2[0] = 0;
19779 cartesian_trajectory_planner_B.iv2[1] = 0;
19780 cartesian_trajectory_planner_B.iv2[2] = 1;
19781 cartesian_trajectory_planner_B.iv2[3] = 0;
19782 cartesian_trajectory_planner_B.iv2[4] = 0;
19783 cartesian_trajectory_planner_B.iv2[5] = 0;
19784 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19785 cartesian_trajectory_planner_B.b_kstr_po < 6;
19786 cartesian_trajectory_planner_B.b_kstr_po++) {
19787 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_po]
19788 =
19789 cartesian_trajectory_planner_B.iv2[cartesian_trajectory_planner_B.b_kstr_po];
19790 }
19791
19792 cartesian_trajectory_planner_B.poslim_data_as[0] = -3.1415926535897931;
19793 cartesian_trajectory_planner_B.poslim_data_as[1] = 3.1415926535897931;
19794 iobj_0->VelocityNumber = 1.0;
19795 iobj_0->PositionNumber = 1.0;
19796 iobj_0->JointAxisInternal[0] = 0.0;
19797 iobj_0->JointAxisInternal[1] = 0.0;
19798 iobj_0->JointAxisInternal[2] = 1.0;
19799 break;
19800
19801 case 1:
19802 cartesian_trajectory_planner_B.iv2[0] = 0;
19803 cartesian_trajectory_planner_B.iv2[1] = 0;
19804 cartesian_trajectory_planner_B.iv2[2] = 0;
19805 cartesian_trajectory_planner_B.iv2[3] = 0;
19806 cartesian_trajectory_planner_B.iv2[4] = 0;
19807 cartesian_trajectory_planner_B.iv2[5] = 1;
19808 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19809 cartesian_trajectory_planner_B.b_kstr_po < 6;
19810 cartesian_trajectory_planner_B.b_kstr_po++) {
19811 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_po]
19812 =
19813 cartesian_trajectory_planner_B.iv2[cartesian_trajectory_planner_B.b_kstr_po];
19814 }
19815
19816 cartesian_trajectory_planner_B.poslim_data_as[0] = -0.5;
19817 cartesian_trajectory_planner_B.poslim_data_as[1] = 0.5;
19818 iobj_0->VelocityNumber = 1.0;
19819 iobj_0->PositionNumber = 1.0;
19820 iobj_0->JointAxisInternal[0] = 0.0;
19821 iobj_0->JointAxisInternal[1] = 0.0;
19822 iobj_0->JointAxisInternal[2] = 1.0;
19823 break;
19824
19825 default:
19826 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19827 cartesian_trajectory_planner_B.b_kstr_po < 6;
19828 cartesian_trajectory_planner_B.b_kstr_po++) {
19829 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_po]
19830 = 0;
19831 }
19832
19833 cartesian_trajectory_planner_B.poslim_data_as[0] = 0.0;
19834 cartesian_trajectory_planner_B.poslim_data_as[1] = 0.0;
19835 iobj_0->VelocityNumber = 0.0;
19836 iobj_0->PositionNumber = 0.0;
19837 iobj_0->JointAxisInternal[0] = 0.0;
19838 iobj_0->JointAxisInternal[1] = 0.0;
19839 iobj_0->JointAxisInternal[2] = 0.0;
19840 break;
19841 }
19842
19843 cartesian_trajectory_planner_B.b_kstr_po = iobj_0->MotionSubspace->size[0] *
19844 iobj_0->MotionSubspace->size[1];
19845 iobj_0->MotionSubspace->size[0] = 6;
19846 iobj_0->MotionSubspace->size[1] = 1;
19847 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace,
19848 cartesian_trajectory_planner_B.b_kstr_po);
19849 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19850 cartesian_trajectory_planner_B.b_kstr_po < 6;
19851 cartesian_trajectory_planner_B.b_kstr_po++) {
19852 iobj_0->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_po] =
19853 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_po];
19854 }
19855
19856 cartesian_trajectory_planner_B.b_kstr_po = iobj_0->
19857 PositionLimitsInternal->size[0] * iobj_0->PositionLimitsInternal->size[1];
19858 iobj_0->PositionLimitsInternal->size[0] = 1;
19859 iobj_0->PositionLimitsInternal->size[1] = 2;
19860 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal,
19861 cartesian_trajectory_planner_B.b_kstr_po);
19862 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19863 cartesian_trajectory_planner_B.b_kstr_po < 2;
19864 cartesian_trajectory_planner_B.b_kstr_po++) {
19865 iobj_0->PositionLimitsInternal->
19866 data[cartesian_trajectory_planner_B.b_kstr_po] =
19867 cartesian_trajectory_planner_B.poslim_data_as[cartesian_trajectory_planner_B.b_kstr_po];
19868 }
19869
19870 cartesian_trajectory_planner_B.b_kstr_po = iobj_0->HomePositionInternal->size
19871 [0];
19872 iobj_0->HomePositionInternal->size[0] = 1;
19873 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal,
19874 cartesian_trajectory_planner_B.b_kstr_po);
19875 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19876 cartesian_trajectory_planner_B.b_kstr_po < 1;
19877 cartesian_trajectory_planner_B.b_kstr_po++) {
19878 iobj_0->HomePositionInternal->data[0] = 0.0;
19879 }
19880
19881 iobj_2->JointInternal = iobj_0;
19882 iobj_2->Index = -1.0;
19883 iobj_2->ParentIndex = -1.0;
19884 cartesian_trajectory_planner_B.b_kstr_po = jtype->size[0] * jtype->size[1];
19885 jtype->size[0] = 1;
19886 jtype->size[1] = obj->JointInternal.Type->size[1];
19887 cart_emxEnsureCapacity_char_T_a(jtype,
19888 cartesian_trajectory_planner_B.b_kstr_po);
19889 cartesian_trajectory_planner_B.loop_ub_ly = obj->JointInternal.Type->size[0] *
19890 obj->JointInternal.Type->size[1] - 1;
19891 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19892 cartesian_trajectory_planner_B.b_kstr_po <=
19893 cartesian_trajectory_planner_B.loop_ub_ly;
19894 cartesian_trajectory_planner_B.b_kstr_po++) {
19895 jtype->data[cartesian_trajectory_planner_B.b_kstr_po] =
19896 obj->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_po];
19897 }
19898
19899 cartesian_traj_emxInit_char_T_a(&jname, 2);
19900 cartesian_trajectory_planner_B.b_kstr_po = jname->size[0] * jname->size[1];
19901 jname->size[0] = 1;
19902 jname->size[1] = obj->JointInternal.NameInternal->size[1];
19903 cart_emxEnsureCapacity_char_T_a(jname,
19904 cartesian_trajectory_planner_B.b_kstr_po);
19905 cartesian_trajectory_planner_B.loop_ub_ly = obj->
19906 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
19907 [1] - 1;
19908 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19909 cartesian_trajectory_planner_B.b_kstr_po <=
19910 cartesian_trajectory_planner_B.loop_ub_ly;
19911 cartesian_trajectory_planner_B.b_kstr_po++) {
19912 jname->data[cartesian_trajectory_planner_B.b_kstr_po] =
19913 obj->JointInternal.NameInternal->
19914 data[cartesian_trajectory_planner_B.b_kstr_po];
19915 }
19916
19917 newjoint = c_rigidBodyJoint_rigidBodyJoint(iobj_1, jname, jtype);
19918 cartesian_trajectory_planner_B.b_kstr_po = jtype->size[0] * jtype->size[1];
19919 jtype->size[0] = 1;
19920 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
19921 cart_emxEnsureCapacity_char_T_a(jtype,
19922 cartesian_trajectory_planner_B.b_kstr_po);
19923 cartesian_trajectory_planner_B.loop_ub_ly = obj->
19924 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
19925 [1] - 1;
19926 cartesian_traj_emxFree_char_T_a(&jname);
19927 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19928 cartesian_trajectory_planner_B.b_kstr_po <=
19929 cartesian_trajectory_planner_B.loop_ub_ly;
19930 cartesian_trajectory_planner_B.b_kstr_po++) {
19931 jtype->data[cartesian_trajectory_planner_B.b_kstr_po] =
19932 obj->JointInternal.NameInternal->
19933 data[cartesian_trajectory_planner_B.b_kstr_po];
19934 }
19935
19936 if (jtype->size[1] != 0) {
19937 cartesian_trajectory_planner_B.b_kstr_po = jtype->size[0] * jtype->size[1];
19938 jtype->size[0] = 1;
19939 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
19940 cart_emxEnsureCapacity_char_T_a(jtype,
19941 cartesian_trajectory_planner_B.b_kstr_po);
19942 cartesian_trajectory_planner_B.loop_ub_ly = obj->
19943 JointInternal.NameInternal->size[0] * obj->
19944 JointInternal.NameInternal->size[1] - 1;
19945 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19946 cartesian_trajectory_planner_B.b_kstr_po <=
19947 cartesian_trajectory_planner_B.loop_ub_ly;
19948 cartesian_trajectory_planner_B.b_kstr_po++) {
19949 jtype->data[cartesian_trajectory_planner_B.b_kstr_po] =
19950 obj->JointInternal.NameInternal->
19951 data[cartesian_trajectory_planner_B.b_kstr_po];
19952 }
19953
19954 if (!newjoint->InTree) {
19955 cartesian_trajectory_planner_B.b_kstr_po = newjoint->NameInternal->size[0]
19956 * newjoint->NameInternal->size[1];
19957 newjoint->NameInternal->size[0] = 1;
19958 newjoint->NameInternal->size[1] = jtype->size[1];
19959 cart_emxEnsureCapacity_char_T_a(newjoint->NameInternal,
19960 cartesian_trajectory_planner_B.b_kstr_po);
19961 cartesian_trajectory_planner_B.loop_ub_ly = jtype->size[0] * jtype->size[1]
19962 - 1;
19963 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19964 cartesian_trajectory_planner_B.b_kstr_po <=
19965 cartesian_trajectory_planner_B.loop_ub_ly;
19966 cartesian_trajectory_planner_B.b_kstr_po++) {
19967 newjoint->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_po] =
19968 jtype->data[cartesian_trajectory_planner_B.b_kstr_po];
19969 }
19970 }
19971 }
19972
19973 cartesian_traj_emxFree_char_T_a(&jtype);
19974 cartesian_trajec_emxInit_real_T(&obj_0, 1);
19975 cartesian_trajectory_planner_B.loop_ub_ly =
19976 obj->JointInternal.PositionLimitsInternal->size[0] *
19977 obj->JointInternal.PositionLimitsInternal->size[1];
19978 cartesian_trajectory_planner_B.b_kstr_po = newjoint->
19979 PositionLimitsInternal->size[0] * newjoint->PositionLimitsInternal->size[1];
19980 newjoint->PositionLimitsInternal->size[0] =
19981 obj->JointInternal.PositionLimitsInternal->size[0];
19982 newjoint->PositionLimitsInternal->size[1] = 2;
19983 cartes_emxEnsureCapacity_real_T(newjoint->PositionLimitsInternal,
19984 cartesian_trajectory_planner_B.b_kstr_po);
19985 cartesian_trajectory_planner_B.b_kstr_po = obj_0->size[0];
19986 obj_0->size[0] = cartesian_trajectory_planner_B.loop_ub_ly;
19987 cartes_emxEnsureCapacity_real_T(obj_0,
19988 cartesian_trajectory_planner_B.b_kstr_po);
19989 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
19990 cartesian_trajectory_planner_B.b_kstr_po <
19991 cartesian_trajectory_planner_B.loop_ub_ly;
19992 cartesian_trajectory_planner_B.b_kstr_po++) {
19993 obj_0->data[cartesian_trajectory_planner_B.b_kstr_po] =
19994 obj->JointInternal.PositionLimitsInternal->
19995 data[cartesian_trajectory_planner_B.b_kstr_po];
19996 }
19997
19998 cartesian_trajectory_planner_B.loop_ub_ly = obj_0->size[0];
19999 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
20000 cartesian_trajectory_planner_B.b_kstr_po <
20001 cartesian_trajectory_planner_B.loop_ub_ly;
20002 cartesian_trajectory_planner_B.b_kstr_po++) {
20003 newjoint->PositionLimitsInternal->
20004 data[cartesian_trajectory_planner_B.b_kstr_po] = obj_0->
20005 data[cartesian_trajectory_planner_B.b_kstr_po];
20006 }
20007
20008 cartesian_trajec_emxFree_real_T(&obj_0);
20009 cartesian_trajec_emxInit_real_T(&obj_1, 1);
20010 cartesian_trajectory_planner_B.b_kstr_po = obj_1->size[0];
20011 obj_1->size[0] = obj->JointInternal.HomePositionInternal->size[0];
20012 cartes_emxEnsureCapacity_real_T(obj_1,
20013 cartesian_trajectory_planner_B.b_kstr_po);
20014 cartesian_trajectory_planner_B.loop_ub_ly =
20015 obj->JointInternal.HomePositionInternal->size[0];
20016 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
20017 cartesian_trajectory_planner_B.b_kstr_po <
20018 cartesian_trajectory_planner_B.loop_ub_ly;
20019 cartesian_trajectory_planner_B.b_kstr_po++) {
20020 obj_1->data[cartesian_trajectory_planner_B.b_kstr_po] =
20021 obj->JointInternal.HomePositionInternal->
20022 data[cartesian_trajectory_planner_B.b_kstr_po];
20023 }
20024
20025 cartesian_trajectory_planner_B.b_kstr_po = newjoint->
20026 HomePositionInternal->size[0];
20027 newjoint->HomePositionInternal->size[0] = obj_1->size[0];
20028 cartes_emxEnsureCapacity_real_T(newjoint->HomePositionInternal,
20029 cartesian_trajectory_planner_B.b_kstr_po);
20030 cartesian_trajectory_planner_B.loop_ub_ly = obj_1->size[0];
20031 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
20032 cartesian_trajectory_planner_B.b_kstr_po <
20033 cartesian_trajectory_planner_B.loop_ub_ly;
20034 cartesian_trajectory_planner_B.b_kstr_po++) {
20035 newjoint->HomePositionInternal->
20036 data[cartesian_trajectory_planner_B.b_kstr_po] = obj_1->
20037 data[cartesian_trajectory_planner_B.b_kstr_po];
20038 }
20039
20040 cartesian_trajec_emxFree_real_T(&obj_1);
20041 cartesian_trajectory_planner_B.obj_idx_0 =
20042 obj->JointInternal.JointAxisInternal[0];
20043 cartesian_trajectory_planner_B.obj_idx_1 =
20044 obj->JointInternal.JointAxisInternal[1];
20045 cartesian_trajectory_planner_B.obj_idx_2 =
20046 obj->JointInternal.JointAxisInternal[2];
20047 newjoint->JointAxisInternal[0] = cartesian_trajectory_planner_B.obj_idx_0;
20048 newjoint->JointAxisInternal[1] = cartesian_trajectory_planner_B.obj_idx_1;
20049 newjoint->JointAxisInternal[2] = cartesian_trajectory_planner_B.obj_idx_2;
20050 cartesian_trajec_emxInit_real_T(&obj_2, 1);
20051 cartesian_trajectory_planner_B.loop_ub_ly = obj->
20052 JointInternal.MotionSubspace->size[0] * obj->
20053 JointInternal.MotionSubspace->size[1];
20054 cartesian_trajectory_planner_B.b_kstr_po = newjoint->MotionSubspace->size[0] *
20055 newjoint->MotionSubspace->size[1];
20056 newjoint->MotionSubspace->size[0] = 6;
20057 newjoint->MotionSubspace->size[1] = obj->JointInternal.MotionSubspace->size[1];
20058 cartes_emxEnsureCapacity_real_T(newjoint->MotionSubspace,
20059 cartesian_trajectory_planner_B.b_kstr_po);
20060 cartesian_trajectory_planner_B.b_kstr_po = obj_2->size[0];
20061 obj_2->size[0] = cartesian_trajectory_planner_B.loop_ub_ly;
20062 cartes_emxEnsureCapacity_real_T(obj_2,
20063 cartesian_trajectory_planner_B.b_kstr_po);
20064 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
20065 cartesian_trajectory_planner_B.b_kstr_po <
20066 cartesian_trajectory_planner_B.loop_ub_ly;
20067 cartesian_trajectory_planner_B.b_kstr_po++) {
20068 obj_2->data[cartesian_trajectory_planner_B.b_kstr_po] =
20069 obj->JointInternal.MotionSubspace->
20070 data[cartesian_trajectory_planner_B.b_kstr_po];
20071 }
20072
20073 cartesian_trajectory_planner_B.loop_ub_ly = obj_2->size[0];
20074 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
20075 cartesian_trajectory_planner_B.b_kstr_po <
20076 cartesian_trajectory_planner_B.loop_ub_ly;
20077 cartesian_trajectory_planner_B.b_kstr_po++) {
20078 newjoint->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_po] =
20079 obj_2->data[cartesian_trajectory_planner_B.b_kstr_po];
20080 }
20081
20082 cartesian_trajec_emxFree_real_T(&obj_2);
20083 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
20084 cartesian_trajectory_planner_B.b_kstr_po < 16;
20085 cartesian_trajectory_planner_B.b_kstr_po++) {
20086 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_po]
20087 = obj->
20088 JointInternal.JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_po];
20089 }
20090
20091 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
20092 cartesian_trajectory_planner_B.b_kstr_po < 16;
20093 cartesian_trajectory_planner_B.b_kstr_po++) {
20094 newjoint->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_po] =
20095 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_po];
20096 }
20097
20098 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
20099 cartesian_trajectory_planner_B.b_kstr_po < 16;
20100 cartesian_trajectory_planner_B.b_kstr_po++) {
20101 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_po]
20102 = obj->
20103 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_po];
20104 }
20105
20106 for (cartesian_trajectory_planner_B.b_kstr_po = 0;
20107 cartesian_trajectory_planner_B.b_kstr_po < 16;
20108 cartesian_trajectory_planner_B.b_kstr_po++) {
20109 newjoint->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_po] =
20110 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_po];
20111 }
20112
20113 iobj_2->JointInternal = newjoint;
20114 return newbody;
20115}
20116
20117static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
20118 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_tra_T
20119 *parentName, c_rigidBodyJoint_cartesian_as_T *iobj_0,
20120 c_rigidBodyJoint_cartesian_as_T *iobj_1, w_robotics_manip_internal_Rig_T
20121 *iobj_2)
20122{
20123 w_robotics_manip_internal_Rig_T *body;
20124 c_rigidBodyJoint_cartesian_as_T *jnt;
20125 emxArray_char_T_cartesian_tra_T *bname;
20126 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
20127
20128 boolean_T exitg1;
20129 int32_T exitg2;
20130 cartesian_traj_emxInit_char_T_a(&bname, 2);
20131 cartesian_trajectory_planner_B.pid = -1.0;
20132 cartesian_trajectory_planner_B.b_kstr_j = bname->size[0] * bname->size[1];
20133 bname->size[0] = 1;
20134 bname->size[1] = obj->Base.NameInternal->size[1];
20135 cart_emxEnsureCapacity_char_T_a(bname, cartesian_trajectory_planner_B.b_kstr_j);
20136 cartesian_trajectory_planner_B.loop_ub_ph = obj->Base.NameInternal->size[0] *
20137 obj->Base.NameInternal->size[1] - 1;
20138 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
20139 cartesian_trajectory_planner_B.b_kstr_j <=
20140 cartesian_trajectory_planner_B.loop_ub_ph;
20141 cartesian_trajectory_planner_B.b_kstr_j++) {
20142 bname->data[cartesian_trajectory_planner_B.b_kstr_j] =
20143 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_j];
20144 }
20145
20146 if (cartesian_trajectory_pla_strcmp(bname, parentName)) {
20147 cartesian_trajectory_planner_B.pid = 0.0;
20148 } else {
20149 cartesian_trajectory_planner_B.b_index_d = obj->NumBodies;
20150 cartesian_trajectory_planner_B.b_i_lc = 0;
20151 exitg1 = false;
20152 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_lc <=
20153 static_cast<int32_T>
20154 (cartesian_trajectory_planner_B.b_index_d) - 1)) {
20155 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_lc];
20156 cartesian_trajectory_planner_B.b_kstr_j = bname->size[0] * bname->size[1];
20157 bname->size[0] = 1;
20158 bname->size[1] = body->NameInternal->size[1];
20159 cart_emxEnsureCapacity_char_T_a(bname,
20160 cartesian_trajectory_planner_B.b_kstr_j);
20161 cartesian_trajectory_planner_B.loop_ub_ph = body->NameInternal->size[0] *
20162 body->NameInternal->size[1] - 1;
20163 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
20164 cartesian_trajectory_planner_B.b_kstr_j <=
20165 cartesian_trajectory_planner_B.loop_ub_ph;
20166 cartesian_trajectory_planner_B.b_kstr_j++) {
20167 bname->data[cartesian_trajectory_planner_B.b_kstr_j] =
20168 body->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_j];
20169 }
20170
20171 if (cartesian_trajectory_pla_strcmp(bname, parentName)) {
20172 cartesian_trajectory_planner_B.pid = static_cast<real_T>
20173 (cartesian_trajectory_planner_B.b_i_lc) + 1.0;
20174 exitg1 = true;
20175 } else {
20176 cartesian_trajectory_planner_B.b_i_lc++;
20177 }
20178 }
20179 }
20180
20181 cartesian_trajectory_planner_B.b_index_d = obj->NumBodies + 1.0;
20182 body = cartesian_trajec_RigidBody_copy(bodyin, iobj_1, iobj_0, iobj_2);
20183 obj->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.b_index_d) - 1]
20184 = body;
20185 body->Index = cartesian_trajectory_planner_B.b_index_d;
20186 body->ParentIndex = cartesian_trajectory_planner_B.pid;
20187 body->JointInternal->InTree = true;
20188 obj->NumBodies++;
20189 jnt = body->JointInternal;
20190 cartesian_trajectory_planner_B.b_kstr_j = bname->size[0] * bname->size[1];
20191 bname->size[0] = 1;
20192 bname->size[1] = jnt->Type->size[1];
20193 cart_emxEnsureCapacity_char_T_a(bname, cartesian_trajectory_planner_B.b_kstr_j);
20194 cartesian_trajectory_planner_B.loop_ub_ph = jnt->Type->size[0] * jnt->
20195 Type->size[1] - 1;
20196 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
20197 cartesian_trajectory_planner_B.b_kstr_j <=
20198 cartesian_trajectory_planner_B.loop_ub_ph;
20199 cartesian_trajectory_planner_B.b_kstr_j++) {
20200 bname->data[cartesian_trajectory_planner_B.b_kstr_j] = jnt->Type->
20201 data[cartesian_trajectory_planner_B.b_kstr_j];
20202 }
20203
20204 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
20205 cartesian_trajectory_planner_B.b_kstr_j < 5;
20206 cartesian_trajectory_planner_B.b_kstr_j++) {
20207 cartesian_trajectory_planner_B.b_f1[cartesian_trajectory_planner_B.b_kstr_j]
20208 = tmp[cartesian_trajectory_planner_B.b_kstr_j];
20209 }
20210
20211 cartesian_trajectory_planner_B.b_bool_n = false;
20212 if (bname->size[1] == 5) {
20213 cartesian_trajectory_planner_B.b_kstr_j = 1;
20214 do {
20215 exitg2 = 0;
20216 if (cartesian_trajectory_planner_B.b_kstr_j - 1 < 5) {
20217 cartesian_trajectory_planner_B.loop_ub_ph =
20218 cartesian_trajectory_planner_B.b_kstr_j - 1;
20219 if (bname->data[cartesian_trajectory_planner_B.loop_ub_ph] !=
20220 cartesian_trajectory_planner_B.b_f1[cartesian_trajectory_planner_B.loop_ub_ph])
20221 {
20222 exitg2 = 1;
20223 } else {
20224 cartesian_trajectory_planner_B.b_kstr_j++;
20225 }
20226 } else {
20227 cartesian_trajectory_planner_B.b_bool_n = true;
20228 exitg2 = 1;
20229 }
20230 } while (exitg2 == 0);
20231 }
20232
20233 cartesian_traj_emxFree_char_T_a(&bname);
20234 if (!cartesian_trajectory_planner_B.b_bool_n) {
20235 obj->NumNonFixedBodies++;
20236 jnt = body->JointInternal;
20237 cartesian_trajectory_planner_B.b_kstr_j = static_cast<int32_T>(body->Index)
20238 - 1;
20239 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_j] =
20240 obj->PositionNumber + 1.0;
20241 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_j + 8] =
20242 obj->PositionNumber + jnt->PositionNumber;
20243 jnt = body->JointInternal;
20244 cartesian_trajectory_planner_B.b_kstr_j = static_cast<int32_T>(body->Index)
20245 - 1;
20246 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_j] =
20247 obj->VelocityNumber + 1.0;
20248 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_j + 8] =
20249 obj->VelocityNumber + jnt->VelocityNumber;
20250 } else {
20251 cartesian_trajectory_planner_B.b_kstr_j = static_cast<int32_T>(body->Index);
20252 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_j - 1] = 0.0;
20253 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_j + 7] = -1.0;
20254 cartesian_trajectory_planner_B.b_kstr_j = static_cast<int32_T>(body->Index);
20255 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_j - 1] = 0.0;
20256 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_j + 7] = -1.0;
20257 }
20258
20259 jnt = body->JointInternal;
20260 obj->PositionNumber += jnt->PositionNumber;
20261 jnt = body->JointInternal;
20262 obj->VelocityNumber += jnt->VelocityNumber;
20263}
20264
20265static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
20266 y_robotics_manip_internal_Rig_T *rigidbodytree,
20267 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
20268 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
20269 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
20270 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
20271 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
20272 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
20273 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
20274 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
20275 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
20276 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
20277 c_rigidBodyJoint_cartesian_as_T *iobj_15, c_rigidBodyJoint_cartesian_as_T
20278 *iobj_16, c_rigidBodyJoint_cartesian_as_T *iobj_17,
20279 c_rigidBodyJoint_cartesian_as_T *iobj_18, c_rigidBodyJoint_cartesian_as_T
20280 *iobj_19, c_rigidBodyJoint_cartesian_as_T *iobj_20,
20281 c_rigidBodyJoint_cartesian_as_T *iobj_21, c_rigidBodyJoint_cartesian_as_T
20282 *iobj_22, c_rigidBodyJoint_cartesian_as_T *iobj_23,
20283 c_rigidBodyJoint_cartesian_as_T *iobj_24, c_rigidBodyJoint_cartesian_as_T
20284 *iobj_25, c_rigidBodyJoint_cartesian_as_T *iobj_26,
20285 c_rigidBodyJoint_cartesian_as_T *iobj_27, c_rigidBodyJoint_cartesian_as_T
20286 *iobj_28, c_rigidBodyJoint_cartesian_as_T *iobj_29,
20287 c_rigidBodyJoint_cartesian_as_T *iobj_30, c_rigidBodyJoint_cartesian_as_T
20288 *iobj_31, c_rigidBodyJoint_cartesian_as_T *iobj_32,
20289 c_rigidBodyJoint_cartesian_as_T *iobj_33, c_rigidBodyJoint_cartesian_as_T
20290 *iobj_34, c_rigidBodyJoint_cartesian_as_T *iobj_35,
20291 c_rigidBodyJoint_cartesian_as_T *iobj_36, c_rigidBodyJoint_cartesian_as_T
20292 *iobj_37, c_rigidBodyJoint_cartesian_as_T *iobj_38,
20293 c_rigidBodyJoint_cartesian_as_T *iobj_39, w_robotics_manip_internal_Rig_T
20294 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41)
20295{
20296 x_robotics_manip_internal_Rig_T *newrobot;
20297 v_robotics_manip_internal_Rig_T *body;
20298 v_robotics_manip_internal_Rig_T *parent;
20299 emxArray_char_T_cartesian_tra_T *b_basename;
20300 w_robotics_manip_internal_Rig_T *body_0;
20301 c_rigidBodyJoint_cartesian_as_T *jnt;
20302 emxArray_char_T_cartesian_tra_T *bname;
20303 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
20304
20305 boolean_T exitg1;
20306 int32_T exitg2;
20307 cartesian_traj_emxInit_char_T_a(&b_basename, 2);
20308 newrobot = RigidBodyTree_RigidBodyTree_ast(iobj_41, iobj_0, iobj_1, iobj_2,
20309 iobj_3, iobj_4, iobj_5, iobj_6, iobj_15, iobj_16, iobj_17, iobj_18, iobj_19,
20310 iobj_20, iobj_21, iobj_22, iobj_39, iobj_40);
20311 cartesian_trajectory_planner_B.b_kstr_i = b_basename->size[0] *
20312 b_basename->size[1];
20313 b_basename->size[0] = 1;
20314 b_basename->size[1] = rigidbodytree->Base.NameInternal->size[1];
20315 cart_emxEnsureCapacity_char_T_a(b_basename,
20316 cartesian_trajectory_planner_B.b_kstr_i);
20317 cartesian_trajectory_planner_B.loop_ub_f4 = rigidbodytree->
20318 Base.NameInternal->size[0] * rigidbodytree->Base.NameInternal->size[1] - 1;
20319 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20320 cartesian_trajectory_planner_B.b_kstr_i <=
20321 cartesian_trajectory_planner_B.loop_ub_f4;
20322 cartesian_trajectory_planner_B.b_kstr_i++) {
20323 b_basename->data[cartesian_trajectory_planner_B.b_kstr_i] =
20324 rigidbodytree->Base.NameInternal->
20325 data[cartesian_trajectory_planner_B.b_kstr_i];
20326 }
20327
20328 cartesian_traj_emxInit_char_T_a(&bname, 2);
20329 cartesian_trajectory_planner_B.bid_h = -1.0;
20330 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20331 bname->size[0] = 1;
20332 bname->size[1] = newrobot->Base.NameInternal->size[1];
20333 cart_emxEnsureCapacity_char_T_a(bname, cartesian_trajectory_planner_B.b_kstr_i);
20334 cartesian_trajectory_planner_B.loop_ub_f4 = newrobot->Base.NameInternal->size
20335 [0] * newrobot->Base.NameInternal->size[1] - 1;
20336 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20337 cartesian_trajectory_planner_B.b_kstr_i <=
20338 cartesian_trajectory_planner_B.loop_ub_f4;
20339 cartesian_trajectory_planner_B.b_kstr_i++) {
20340 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20341 newrobot->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20342 }
20343
20344 if (cartesian_trajectory_pla_strcmp(bname, b_basename)) {
20345 cartesian_trajectory_planner_B.bid_h = 0.0;
20346 } else {
20347 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies;
20348 cartesian_trajectory_planner_B.b_i_f = 0;
20349 exitg1 = false;
20350 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_f <=
20351 static_cast<int32_T>
20352 (cartesian_trajectory_planner_B.b_index) - 1)) {
20353 body_0 = newrobot->Bodies[cartesian_trajectory_planner_B.b_i_f];
20354 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20355 bname->size[0] = 1;
20356 bname->size[1] = body_0->NameInternal->size[1];
20357 cart_emxEnsureCapacity_char_T_a(bname,
20358 cartesian_trajectory_planner_B.b_kstr_i);
20359 cartesian_trajectory_planner_B.loop_ub_f4 = body_0->NameInternal->size[0] *
20360 body_0->NameInternal->size[1] - 1;
20361 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20362 cartesian_trajectory_planner_B.b_kstr_i <=
20363 cartesian_trajectory_planner_B.loop_ub_f4;
20364 cartesian_trajectory_planner_B.b_kstr_i++) {
20365 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20366 body_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20367 }
20368
20369 if (cartesian_trajectory_pla_strcmp(bname, b_basename)) {
20370 cartesian_trajectory_planner_B.bid_h = static_cast<real_T>
20371 (cartesian_trajectory_planner_B.b_i_f) + 1.0;
20372 exitg1 = true;
20373 } else {
20374 cartesian_trajectory_planner_B.b_i_f++;
20375 }
20376 }
20377 }
20378
20379 if ((!(cartesian_trajectory_planner_B.bid_h == 0.0)) &&
20380 (cartesian_trajectory_planner_B.bid_h < 0.0)) {
20381 cartesian_trajectory_planner_B.b_kstr_i = newrobot->Base.NameInternal->size
20382 [0] * newrobot->Base.NameInternal->size[1];
20383 newrobot->Base.NameInternal->size[0] = 1;
20384 newrobot->Base.NameInternal->size[1] = b_basename->size[1];
20385 cart_emxEnsureCapacity_char_T_a(newrobot->Base.NameInternal,
20386 cartesian_trajectory_planner_B.b_kstr_i);
20387 cartesian_trajectory_planner_B.loop_ub_f4 = b_basename->size[0] *
20388 b_basename->size[1] - 1;
20389 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20390 cartesian_trajectory_planner_B.b_kstr_i <=
20391 cartesian_trajectory_planner_B.loop_ub_f4;
20392 cartesian_trajectory_planner_B.b_kstr_i++) {
20393 newrobot->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i]
20394 = b_basename->data[cartesian_trajectory_planner_B.b_kstr_i];
20395 }
20396 }
20397
20398 if (1.0 <= rigidbodytree->NumBodies) {
20399 body = rigidbodytree->Bodies[0];
20400 cartesian_trajectory_planner_B.bid_h = body->ParentIndex;
20401 if (cartesian_trajectory_planner_B.bid_h > 0.0) {
20402 parent = rigidbodytree->Bodies[static_cast<int32_T>
20403 (cartesian_trajectory_planner_B.bid_h) - 1];
20404 } else {
20405 parent = &rigidbodytree->Base;
20406 }
20407
20408 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20409 bname->size[0] = 1;
20410 bname->size[1] = parent->NameInternal->size[1];
20411 cart_emxEnsureCapacity_char_T_a(bname,
20412 cartesian_trajectory_planner_B.b_kstr_i);
20413 cartesian_trajectory_planner_B.loop_ub_f4 = parent->NameInternal->size[0] *
20414 parent->NameInternal->size[1] - 1;
20415 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20416 cartesian_trajectory_planner_B.b_kstr_i <=
20417 cartesian_trajectory_planner_B.loop_ub_f4;
20418 cartesian_trajectory_planner_B.b_kstr_i++) {
20419 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20420 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20421 }
20422
20423 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_24, iobj_23,
20424 iobj_7);
20425 }
20426
20427 if (2.0 <= rigidbodytree->NumBodies) {
20428 body = rigidbodytree->Bodies[1];
20429 cartesian_trajectory_planner_B.bid_h = body->ParentIndex;
20430 if (cartesian_trajectory_planner_B.bid_h > 0.0) {
20431 parent = rigidbodytree->Bodies[static_cast<int32_T>
20432 (cartesian_trajectory_planner_B.bid_h) - 1];
20433 } else {
20434 parent = &rigidbodytree->Base;
20435 }
20436
20437 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20438 bname->size[0] = 1;
20439 bname->size[1] = parent->NameInternal->size[1];
20440 cart_emxEnsureCapacity_char_T_a(bname,
20441 cartesian_trajectory_planner_B.b_kstr_i);
20442 cartesian_trajectory_planner_B.loop_ub_f4 = parent->NameInternal->size[0] *
20443 parent->NameInternal->size[1] - 1;
20444 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20445 cartesian_trajectory_planner_B.b_kstr_i <=
20446 cartesian_trajectory_planner_B.loop_ub_f4;
20447 cartesian_trajectory_planner_B.b_kstr_i++) {
20448 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20449 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20450 }
20451
20452 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_26, iobj_25,
20453 iobj_8);
20454 }
20455
20456 if (3.0 <= rigidbodytree->NumBodies) {
20457 body = rigidbodytree->Bodies[2];
20458 cartesian_trajectory_planner_B.bid_h = body->ParentIndex;
20459 if (cartesian_trajectory_planner_B.bid_h > 0.0) {
20460 parent = rigidbodytree->Bodies[static_cast<int32_T>
20461 (cartesian_trajectory_planner_B.bid_h) - 1];
20462 } else {
20463 parent = &rigidbodytree->Base;
20464 }
20465
20466 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20467 bname->size[0] = 1;
20468 bname->size[1] = parent->NameInternal->size[1];
20469 cart_emxEnsureCapacity_char_T_a(bname,
20470 cartesian_trajectory_planner_B.b_kstr_i);
20471 cartesian_trajectory_planner_B.loop_ub_f4 = parent->NameInternal->size[0] *
20472 parent->NameInternal->size[1] - 1;
20473 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20474 cartesian_trajectory_planner_B.b_kstr_i <=
20475 cartesian_trajectory_planner_B.loop_ub_f4;
20476 cartesian_trajectory_planner_B.b_kstr_i++) {
20477 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20478 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20479 }
20480
20481 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_28, iobj_27,
20482 iobj_9);
20483 }
20484
20485 if (4.0 <= rigidbodytree->NumBodies) {
20486 body = rigidbodytree->Bodies[3];
20487 cartesian_trajectory_planner_B.bid_h = body->ParentIndex;
20488 if (cartesian_trajectory_planner_B.bid_h > 0.0) {
20489 parent = rigidbodytree->Bodies[static_cast<int32_T>
20490 (cartesian_trajectory_planner_B.bid_h) - 1];
20491 } else {
20492 parent = &rigidbodytree->Base;
20493 }
20494
20495 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20496 bname->size[0] = 1;
20497 bname->size[1] = parent->NameInternal->size[1];
20498 cart_emxEnsureCapacity_char_T_a(bname,
20499 cartesian_trajectory_planner_B.b_kstr_i);
20500 cartesian_trajectory_planner_B.loop_ub_f4 = parent->NameInternal->size[0] *
20501 parent->NameInternal->size[1] - 1;
20502 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20503 cartesian_trajectory_planner_B.b_kstr_i <=
20504 cartesian_trajectory_planner_B.loop_ub_f4;
20505 cartesian_trajectory_planner_B.b_kstr_i++) {
20506 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20507 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20508 }
20509
20510 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_30, iobj_29,
20511 iobj_10);
20512 }
20513
20514 if (5.0 <= rigidbodytree->NumBodies) {
20515 body = rigidbodytree->Bodies[4];
20516 cartesian_trajectory_planner_B.bid_h = body->ParentIndex;
20517 if (cartesian_trajectory_planner_B.bid_h > 0.0) {
20518 parent = rigidbodytree->Bodies[static_cast<int32_T>
20519 (cartesian_trajectory_planner_B.bid_h) - 1];
20520 } else {
20521 parent = &rigidbodytree->Base;
20522 }
20523
20524 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20525 bname->size[0] = 1;
20526 bname->size[1] = parent->NameInternal->size[1];
20527 cart_emxEnsureCapacity_char_T_a(bname,
20528 cartesian_trajectory_planner_B.b_kstr_i);
20529 cartesian_trajectory_planner_B.loop_ub_f4 = parent->NameInternal->size[0] *
20530 parent->NameInternal->size[1] - 1;
20531 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20532 cartesian_trajectory_planner_B.b_kstr_i <=
20533 cartesian_trajectory_planner_B.loop_ub_f4;
20534 cartesian_trajectory_planner_B.b_kstr_i++) {
20535 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20536 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20537 }
20538
20539 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_32, iobj_31,
20540 iobj_11);
20541 }
20542
20543 if (6.0 <= rigidbodytree->NumBodies) {
20544 body = rigidbodytree->Bodies[5];
20545 cartesian_trajectory_planner_B.bid_h = body->ParentIndex;
20546 if (cartesian_trajectory_planner_B.bid_h > 0.0) {
20547 parent = rigidbodytree->Bodies[static_cast<int32_T>
20548 (cartesian_trajectory_planner_B.bid_h) - 1];
20549 } else {
20550 parent = &rigidbodytree->Base;
20551 }
20552
20553 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20554 bname->size[0] = 1;
20555 bname->size[1] = parent->NameInternal->size[1];
20556 cart_emxEnsureCapacity_char_T_a(bname,
20557 cartesian_trajectory_planner_B.b_kstr_i);
20558 cartesian_trajectory_planner_B.loop_ub_f4 = parent->NameInternal->size[0] *
20559 parent->NameInternal->size[1] - 1;
20560 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20561 cartesian_trajectory_planner_B.b_kstr_i <=
20562 cartesian_trajectory_planner_B.loop_ub_f4;
20563 cartesian_trajectory_planner_B.b_kstr_i++) {
20564 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20565 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20566 }
20567
20568 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_34, iobj_33,
20569 iobj_12);
20570 }
20571
20572 if (7.0 <= rigidbodytree->NumBodies) {
20573 body = rigidbodytree->Bodies[6];
20574 cartesian_trajectory_planner_B.bid_h = body->ParentIndex;
20575 if (cartesian_trajectory_planner_B.bid_h > 0.0) {
20576 parent = rigidbodytree->Bodies[static_cast<int32_T>
20577 (cartesian_trajectory_planner_B.bid_h) - 1];
20578 } else {
20579 parent = &rigidbodytree->Base;
20580 }
20581
20582 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20583 bname->size[0] = 1;
20584 bname->size[1] = parent->NameInternal->size[1];
20585 cart_emxEnsureCapacity_char_T_a(bname,
20586 cartesian_trajectory_planner_B.b_kstr_i);
20587 cartesian_trajectory_planner_B.loop_ub_f4 = parent->NameInternal->size[0] *
20588 parent->NameInternal->size[1] - 1;
20589 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20590 cartesian_trajectory_planner_B.b_kstr_i <=
20591 cartesian_trajectory_planner_B.loop_ub_f4;
20592 cartesian_trajectory_planner_B.b_kstr_i++) {
20593 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20594 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20595 }
20596
20597 cartesian_trajectory_planner_B.bid_h = -1.0;
20598 cartesian_trajectory_planner_B.b_kstr_i = b_basename->size[0] *
20599 b_basename->size[1];
20600 b_basename->size[0] = 1;
20601 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
20602 cart_emxEnsureCapacity_char_T_a(b_basename,
20603 cartesian_trajectory_planner_B.b_kstr_i);
20604 cartesian_trajectory_planner_B.loop_ub_f4 = newrobot->
20605 Base.NameInternal->size[0] * newrobot->Base.NameInternal->size[1] - 1;
20606 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20607 cartesian_trajectory_planner_B.b_kstr_i <=
20608 cartesian_trajectory_planner_B.loop_ub_f4;
20609 cartesian_trajectory_planner_B.b_kstr_i++) {
20610 b_basename->data[cartesian_trajectory_planner_B.b_kstr_i] =
20611 newrobot->Base.NameInternal->
20612 data[cartesian_trajectory_planner_B.b_kstr_i];
20613 }
20614
20615 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
20616 cartesian_trajectory_planner_B.bid_h = 0.0;
20617 } else {
20618 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies;
20619 cartesian_trajectory_planner_B.b_i_f = 0;
20620 exitg1 = false;
20621 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_f <=
20622 static_cast<int32_T>
20623 (cartesian_trajectory_planner_B.b_index) - 1)) {
20624 body_0 = newrobot->Bodies[cartesian_trajectory_planner_B.b_i_f];
20625 cartesian_trajectory_planner_B.b_kstr_i = b_basename->size[0] *
20626 b_basename->size[1];
20627 b_basename->size[0] = 1;
20628 b_basename->size[1] = body_0->NameInternal->size[1];
20629 cart_emxEnsureCapacity_char_T_a(b_basename,
20630 cartesian_trajectory_planner_B.b_kstr_i);
20631 cartesian_trajectory_planner_B.loop_ub_f4 = body_0->NameInternal->size[0]
20632 * body_0->NameInternal->size[1] - 1;
20633 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20634 cartesian_trajectory_planner_B.b_kstr_i <=
20635 cartesian_trajectory_planner_B.loop_ub_f4;
20636 cartesian_trajectory_planner_B.b_kstr_i++) {
20637 b_basename->data[cartesian_trajectory_planner_B.b_kstr_i] =
20638 body_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20639 }
20640
20641 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
20642 cartesian_trajectory_planner_B.bid_h = static_cast<real_T>
20643 (cartesian_trajectory_planner_B.b_i_f) + 1.0;
20644 exitg1 = true;
20645 } else {
20646 cartesian_trajectory_planner_B.b_i_f++;
20647 }
20648 }
20649 }
20650
20651 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies + 1.0;
20652 body_0 = cartesian_trajec_RigidBody_copy(body, iobj_35, iobj_36, iobj_13);
20653 newrobot->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.b_index)
20654 - 1] = body_0;
20655 body_0->Index = cartesian_trajectory_planner_B.b_index;
20656 body_0->ParentIndex = cartesian_trajectory_planner_B.bid_h;
20657 body_0->JointInternal->InTree = true;
20658 newrobot->NumBodies++;
20659 jnt = body_0->JointInternal;
20660 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20661 bname->size[0] = 1;
20662 bname->size[1] = jnt->Type->size[1];
20663 cart_emxEnsureCapacity_char_T_a(bname,
20664 cartesian_trajectory_planner_B.b_kstr_i);
20665 cartesian_trajectory_planner_B.loop_ub_f4 = jnt->Type->size[0] * jnt->
20666 Type->size[1] - 1;
20667 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20668 cartesian_trajectory_planner_B.b_kstr_i <=
20669 cartesian_trajectory_planner_B.loop_ub_f4;
20670 cartesian_trajectory_planner_B.b_kstr_i++) {
20671 bname->data[cartesian_trajectory_planner_B.b_kstr_i] = jnt->Type->
20672 data[cartesian_trajectory_planner_B.b_kstr_i];
20673 }
20674
20675 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20676 cartesian_trajectory_planner_B.b_kstr_i < 5;
20677 cartesian_trajectory_planner_B.b_kstr_i++) {
20678 cartesian_trajectory_planner_B.b_oo[cartesian_trajectory_planner_B.b_kstr_i]
20679 = tmp[cartesian_trajectory_planner_B.b_kstr_i];
20680 }
20681
20682 cartesian_trajectory_planner_B.b_bool_k = false;
20683 if (bname->size[1] == 5) {
20684 cartesian_trajectory_planner_B.b_kstr_i = 1;
20685 do {
20686 exitg2 = 0;
20687 if (cartesian_trajectory_planner_B.b_kstr_i - 1 < 5) {
20688 cartesian_trajectory_planner_B.loop_ub_f4 =
20689 cartesian_trajectory_planner_B.b_kstr_i - 1;
20690 if (bname->data[cartesian_trajectory_planner_B.loop_ub_f4] !=
20691 cartesian_trajectory_planner_B.b_oo[cartesian_trajectory_planner_B.loop_ub_f4])
20692 {
20693 exitg2 = 1;
20694 } else {
20695 cartesian_trajectory_planner_B.b_kstr_i++;
20696 }
20697 } else {
20698 cartesian_trajectory_planner_B.b_bool_k = true;
20699 exitg2 = 1;
20700 }
20701 } while (exitg2 == 0);
20702 }
20703
20704 if (!cartesian_trajectory_planner_B.b_bool_k) {
20705 newrobot->NumNonFixedBodies++;
20706 jnt = body_0->JointInternal;
20707 cartesian_trajectory_planner_B.b_kstr_i = static_cast<int32_T>
20708 (body_0->Index) - 1;
20709 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_i] =
20710 newrobot->PositionNumber + 1.0;
20711 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_i + 8] =
20712 newrobot->PositionNumber + jnt->PositionNumber;
20713 jnt = body_0->JointInternal;
20714 cartesian_trajectory_planner_B.b_kstr_i = static_cast<int32_T>
20715 (body_0->Index) - 1;
20716 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_i] =
20717 newrobot->VelocityNumber + 1.0;
20718 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_i + 8] =
20719 newrobot->VelocityNumber + jnt->VelocityNumber;
20720 } else {
20721 cartesian_trajectory_planner_B.b_kstr_i = static_cast<int32_T>
20722 (body_0->Index);
20723 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_i - 1] =
20724 0.0;
20725 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_i + 7] =
20726 -1.0;
20727 cartesian_trajectory_planner_B.b_kstr_i = static_cast<int32_T>
20728 (body_0->Index);
20729 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_i - 1] =
20730 0.0;
20731 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_i + 7] =
20732 -1.0;
20733 }
20734
20735 jnt = body_0->JointInternal;
20736 newrobot->PositionNumber += jnt->PositionNumber;
20737 jnt = body_0->JointInternal;
20738 newrobot->VelocityNumber += jnt->VelocityNumber;
20739 }
20740
20741 if (8.0 <= rigidbodytree->NumBodies) {
20742 body = rigidbodytree->Bodies[7];
20743 cartesian_trajectory_planner_B.bid_h = body->ParentIndex;
20744 if (cartesian_trajectory_planner_B.bid_h > 0.0) {
20745 parent = rigidbodytree->Bodies[static_cast<int32_T>
20746 (cartesian_trajectory_planner_B.bid_h) - 1];
20747 } else {
20748 parent = &rigidbodytree->Base;
20749 }
20750
20751 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20752 bname->size[0] = 1;
20753 bname->size[1] = parent->NameInternal->size[1];
20754 cart_emxEnsureCapacity_char_T_a(bname,
20755 cartesian_trajectory_planner_B.b_kstr_i);
20756 cartesian_trajectory_planner_B.loop_ub_f4 = parent->NameInternal->size[0] *
20757 parent->NameInternal->size[1] - 1;
20758 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20759 cartesian_trajectory_planner_B.b_kstr_i <=
20760 cartesian_trajectory_planner_B.loop_ub_f4;
20761 cartesian_trajectory_planner_B.b_kstr_i++) {
20762 bname->data[cartesian_trajectory_planner_B.b_kstr_i] =
20763 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20764 }
20765
20766 cartesian_trajectory_planner_B.bid_h = -1.0;
20767 cartesian_trajectory_planner_B.b_kstr_i = b_basename->size[0] *
20768 b_basename->size[1];
20769 b_basename->size[0] = 1;
20770 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
20771 cart_emxEnsureCapacity_char_T_a(b_basename,
20772 cartesian_trajectory_planner_B.b_kstr_i);
20773 cartesian_trajectory_planner_B.loop_ub_f4 = newrobot->
20774 Base.NameInternal->size[0] * newrobot->Base.NameInternal->size[1] - 1;
20775 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20776 cartesian_trajectory_planner_B.b_kstr_i <=
20777 cartesian_trajectory_planner_B.loop_ub_f4;
20778 cartesian_trajectory_planner_B.b_kstr_i++) {
20779 b_basename->data[cartesian_trajectory_planner_B.b_kstr_i] =
20780 newrobot->Base.NameInternal->
20781 data[cartesian_trajectory_planner_B.b_kstr_i];
20782 }
20783
20784 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
20785 cartesian_trajectory_planner_B.bid_h = 0.0;
20786 } else {
20787 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies;
20788 cartesian_trajectory_planner_B.b_i_f = 0;
20789 exitg1 = false;
20790 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_f <=
20791 static_cast<int32_T>
20792 (cartesian_trajectory_planner_B.b_index) - 1)) {
20793 body_0 = newrobot->Bodies[cartesian_trajectory_planner_B.b_i_f];
20794 cartesian_trajectory_planner_B.b_kstr_i = b_basename->size[0] *
20795 b_basename->size[1];
20796 b_basename->size[0] = 1;
20797 b_basename->size[1] = body_0->NameInternal->size[1];
20798 cart_emxEnsureCapacity_char_T_a(b_basename,
20799 cartesian_trajectory_planner_B.b_kstr_i);
20800 cartesian_trajectory_planner_B.loop_ub_f4 = body_0->NameInternal->size[0]
20801 * body_0->NameInternal->size[1] - 1;
20802 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20803 cartesian_trajectory_planner_B.b_kstr_i <=
20804 cartesian_trajectory_planner_B.loop_ub_f4;
20805 cartesian_trajectory_planner_B.b_kstr_i++) {
20806 b_basename->data[cartesian_trajectory_planner_B.b_kstr_i] =
20807 body_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_i];
20808 }
20809
20810 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
20811 cartesian_trajectory_planner_B.bid_h = static_cast<real_T>
20812 (cartesian_trajectory_planner_B.b_i_f) + 1.0;
20813 exitg1 = true;
20814 } else {
20815 cartesian_trajectory_planner_B.b_i_f++;
20816 }
20817 }
20818 }
20819
20820 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies + 1.0;
20821 body_0 = cartesian_trajec_RigidBody_copy(body, iobj_37, iobj_38, iobj_14);
20822 newrobot->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.b_index)
20823 - 1] = body_0;
20824 body_0->Index = cartesian_trajectory_planner_B.b_index;
20825 body_0->ParentIndex = cartesian_trajectory_planner_B.bid_h;
20826 body_0->JointInternal->InTree = true;
20827 newrobot->NumBodies++;
20828 jnt = body_0->JointInternal;
20829 cartesian_trajectory_planner_B.b_kstr_i = bname->size[0] * bname->size[1];
20830 bname->size[0] = 1;
20831 bname->size[1] = jnt->Type->size[1];
20832 cart_emxEnsureCapacity_char_T_a(bname,
20833 cartesian_trajectory_planner_B.b_kstr_i);
20834 cartesian_trajectory_planner_B.loop_ub_f4 = jnt->Type->size[0] * jnt->
20835 Type->size[1] - 1;
20836 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20837 cartesian_trajectory_planner_B.b_kstr_i <=
20838 cartesian_trajectory_planner_B.loop_ub_f4;
20839 cartesian_trajectory_planner_B.b_kstr_i++) {
20840 bname->data[cartesian_trajectory_planner_B.b_kstr_i] = jnt->Type->
20841 data[cartesian_trajectory_planner_B.b_kstr_i];
20842 }
20843
20844 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
20845 cartesian_trajectory_planner_B.b_kstr_i < 5;
20846 cartesian_trajectory_planner_B.b_kstr_i++) {
20847 cartesian_trajectory_planner_B.b_oo[cartesian_trajectory_planner_B.b_kstr_i]
20848 = tmp[cartesian_trajectory_planner_B.b_kstr_i];
20849 }
20850
20851 cartesian_trajectory_planner_B.b_bool_k = false;
20852 if (bname->size[1] == 5) {
20853 cartesian_trajectory_planner_B.b_kstr_i = 1;
20854 do {
20855 exitg2 = 0;
20856 if (cartesian_trajectory_planner_B.b_kstr_i - 1 < 5) {
20857 cartesian_trajectory_planner_B.loop_ub_f4 =
20858 cartesian_trajectory_planner_B.b_kstr_i - 1;
20859 if (bname->data[cartesian_trajectory_planner_B.loop_ub_f4] !=
20860 cartesian_trajectory_planner_B.b_oo[cartesian_trajectory_planner_B.loop_ub_f4])
20861 {
20862 exitg2 = 1;
20863 } else {
20864 cartesian_trajectory_planner_B.b_kstr_i++;
20865 }
20866 } else {
20867 cartesian_trajectory_planner_B.b_bool_k = true;
20868 exitg2 = 1;
20869 }
20870 } while (exitg2 == 0);
20871 }
20872
20873 if (!cartesian_trajectory_planner_B.b_bool_k) {
20874 newrobot->NumNonFixedBodies++;
20875 jnt = body_0->JointInternal;
20876 cartesian_trajectory_planner_B.b_kstr_i = static_cast<int32_T>
20877 (body_0->Index) - 1;
20878 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_i] =
20879 newrobot->PositionNumber + 1.0;
20880 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_i + 8] =
20881 newrobot->PositionNumber + jnt->PositionNumber;
20882 jnt = body_0->JointInternal;
20883 cartesian_trajectory_planner_B.b_kstr_i = static_cast<int32_T>
20884 (body_0->Index) - 1;
20885 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_i] =
20886 newrobot->VelocityNumber + 1.0;
20887 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_i + 8] =
20888 newrobot->VelocityNumber + jnt->VelocityNumber;
20889 } else {
20890 cartesian_trajectory_planner_B.b_kstr_i = static_cast<int32_T>
20891 (body_0->Index);
20892 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_i - 1] =
20893 0.0;
20894 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_i + 7] =
20895 -1.0;
20896 cartesian_trajectory_planner_B.b_kstr_i = static_cast<int32_T>
20897 (body_0->Index);
20898 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_i - 1] =
20899 0.0;
20900 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_i + 7] =
20901 -1.0;
20902 }
20903
20904 jnt = body_0->JointInternal;
20905 newrobot->PositionNumber += jnt->PositionNumber;
20906 jnt = body_0->JointInternal;
20907 newrobot->VelocityNumber += jnt->VelocityNumber;
20908 }
20909
20910 cartesian_traj_emxFree_char_T_a(&bname);
20911 cartesian_traj_emxFree_char_T_a(&b_basename);
20912 obj->RigidBodyTreeInternal = newrobot;
20913}
20914
20915static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
20916 (h_robotics_core_internal_Damp_T *obj)
20917{
20918 h_robotics_core_internal_Damp_T *b_obj;
20919 int32_T i;
20920 static const char_T tmp[22] = { 'B', 'F', 'G', 'S', 'G', 'r', 'a', 'd', 'i',
20921 'e', 'n', 't', 'P', 'r', 'o', 'j', 'e', 'c', 't', 'i', 'o', 'n' };
20922
20923 b_obj = obj;
20924 obj->MaxNumIteration = 1500.0;
20925 obj->MaxTime = 10.0;
20926 obj->GradientTolerance = 1.0E-7;
20927 obj->SolutionTolerance = 1.0E-6;
20928 obj->ArmijoRuleBeta = 0.4;
20929 obj->ArmijoRuleSigma = 1.0E-5;
20930 obj->ConstraintsOn = true;
20931 obj->RandomRestart = true;
20932 obj->StepTolerance = 1.0E-14;
20933 for (i = 0; i < 22; i++) {
20934 obj->Name[i] = tmp[i];
20935 }
20936
20937 obj->ConstraintMatrix->size[0] = 0;
20938 obj->ConstraintMatrix->size[1] = 0;
20939 obj->ConstraintBound->size[0] = 0;
20940 obj->TimeObj.StartTime = -1.0;
20941 obj->TimeObjInternal.StartTime = -1.0;
20942 return b_obj;
20943}
20944
20945static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__a_T
20946 *pStruct)
20947{
20948 cartesian_traj_emxInit_char_T_a(&pStruct->Type, 2);
20949 cartesian_trajec_emxInit_real_T(&pStruct->MotionSubspace, 2);
20950}
20951
20952static void emxInitStruct_o_robotics_mani_a(o_robotics_manip_internal_R_a_T
20953 *pStruct)
20954{
20955 cartesian_traj_emxInit_char_T_a(&pStruct->NameInternal, 2);
20956 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
20957}
20958
20959static void emxInitStruct_p_robotics_mani_a(p_robotics_manip_internal_R_a_T
20960 *pStruct)
20961{
20962 emxInitStruct_o_robotics_mani_a(&pStruct->Base);
20963}
20964
20965static void emxInitStruct_robotics_slman_as(robotics_slmanip_internal_b_a_T
20966 *pStruct)
20967{
20968 emxInitStruct_p_robotics_mani_a(&pStruct->TreeInternal);
20969}
20970
20971static void emxInitStruct_n_robotics_mani_a(n_robotics_manip_internal_R_a_T
20972 *pStruct)
20973{
20974 cartesian_traj_emxInit_char_T_a(&pStruct->NameInternal, 2);
20975 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
20976}
20977
20978static n_robotics_manip_internal_R_a_T *cartesian_RigidBody_RigidBody_a
20979 (n_robotics_manip_internal_R_a_T *obj)
20980{
20981 n_robotics_manip_internal_R_a_T *b_obj;
20982 int8_T msubspace_data[36];
20983 emxArray_char_T_cartesian_tra_T *switch_expression;
20984 boolean_T b_bool;
20985 int32_T b_kstr;
20986 char_T b[8];
20987 char_T b_0[9];
20988 int32_T loop_ub;
20989 int8_T tmp[6];
20990 static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
20991 'l', 'i', 'n', 'k' };
20992
20993 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
20994
20995 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20996
20997 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20998
20999 static const real_T tmp_4[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21000 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21001
21002 static const real_T tmp_5[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21003 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21004
21005 int32_T exitg1;
21006 b_obj = obj;
21007 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21008 obj->NameInternal->size[0] = 1;
21009 obj->NameInternal->size[1] = 13;
21010 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
21011 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
21012 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
21013 }
21014
21015 obj->ParentIndex = 0.0;
21016 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21017 obj->JointInternal.Type->size[0] = 1;
21018 obj->JointInternal.Type->size[1] = 5;
21019 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
21020 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
21021 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
21022 }
21023
21024 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
21025 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21026 switch_expression->size[0] = 1;
21027 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21028 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
21029 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21030 - 1;
21031 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21032 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21033 }
21034
21035 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21036 b[b_kstr] = tmp_2[b_kstr];
21037 }
21038
21039 b_bool = false;
21040 if (switch_expression->size[1] == 8) {
21041 b_kstr = 1;
21042 do {
21043 exitg1 = 0;
21044 if (b_kstr - 1 < 8) {
21045 loop_ub = b_kstr - 1;
21046 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21047 exitg1 = 1;
21048 } else {
21049 b_kstr++;
21050 }
21051 } else {
21052 b_bool = true;
21053 exitg1 = 1;
21054 }
21055 } while (exitg1 == 0);
21056 }
21057
21058 if (b_bool) {
21059 b_kstr = 0;
21060 } else {
21061 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21062 b_0[b_kstr] = tmp_3[b_kstr];
21063 }
21064
21065 b_bool = false;
21066 if (switch_expression->size[1] == 9) {
21067 b_kstr = 1;
21068 do {
21069 exitg1 = 0;
21070 if (b_kstr - 1 < 9) {
21071 loop_ub = b_kstr - 1;
21072 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21073 exitg1 = 1;
21074 } else {
21075 b_kstr++;
21076 }
21077 } else {
21078 b_bool = true;
21079 exitg1 = 1;
21080 }
21081 } while (exitg1 == 0);
21082 }
21083
21084 if (b_bool) {
21085 b_kstr = 1;
21086 } else {
21087 b_kstr = -1;
21088 }
21089 }
21090
21091 cartesian_traj_emxFree_char_T_a(&switch_expression);
21092 switch (b_kstr) {
21093 case 0:
21094 tmp[0] = 0;
21095 tmp[1] = 0;
21096 tmp[2] = 1;
21097 tmp[3] = 0;
21098 tmp[4] = 0;
21099 tmp[5] = 0;
21100 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21101 msubspace_data[b_kstr] = tmp[b_kstr];
21102 }
21103
21104 obj->JointInternal.PositionNumber = 1.0;
21105 obj->JointInternal.JointAxisInternal[0] = 0.0;
21106 obj->JointInternal.JointAxisInternal[1] = 0.0;
21107 obj->JointInternal.JointAxisInternal[2] = 1.0;
21108 break;
21109
21110 case 1:
21111 tmp[0] = 0;
21112 tmp[1] = 0;
21113 tmp[2] = 0;
21114 tmp[3] = 0;
21115 tmp[4] = 0;
21116 tmp[5] = 1;
21117 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21118 msubspace_data[b_kstr] = tmp[b_kstr];
21119 }
21120
21121 obj->JointInternal.PositionNumber = 1.0;
21122 obj->JointInternal.JointAxisInternal[0] = 0.0;
21123 obj->JointInternal.JointAxisInternal[1] = 0.0;
21124 obj->JointInternal.JointAxisInternal[2] = 1.0;
21125 break;
21126
21127 default:
21128 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21129 msubspace_data[b_kstr] = 0;
21130 }
21131
21132 obj->JointInternal.PositionNumber = 0.0;
21133 obj->JointInternal.JointAxisInternal[0] = 0.0;
21134 obj->JointInternal.JointAxisInternal[1] = 0.0;
21135 obj->JointInternal.JointAxisInternal[2] = 0.0;
21136 break;
21137 }
21138
21139 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21140 obj->JointInternal.MotionSubspace->size[1];
21141 obj->JointInternal.MotionSubspace->size[0] = 6;
21142 obj->JointInternal.MotionSubspace->size[1] = 1;
21143 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21144 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21145 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
21146 }
21147
21148 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21149 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_4[b_kstr];
21150 }
21151
21152 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21153 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_5[b_kstr];
21154 }
21155
21156 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21157 obj->JointInternal.MotionSubspace->size[1];
21158 obj->JointInternal.MotionSubspace->size[0] = 6;
21159 obj->JointInternal.MotionSubspace->size[1] = 1;
21160 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21161 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21162 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
21163 }
21164
21165 obj->JointInternal.JointAxisInternal[0] = 0.0;
21166 obj->JointInternal.JointAxisInternal[1] = 0.0;
21167 obj->JointInternal.JointAxisInternal[2] = 0.0;
21168 return b_obj;
21169}
21170
21171static n_robotics_manip_internal_R_a_T *cartesia_RigidBody_RigidBody_as
21172 (n_robotics_manip_internal_R_a_T *obj)
21173{
21174 n_robotics_manip_internal_R_a_T *b_obj;
21175 int8_T msubspace_data[36];
21176 emxArray_char_T_cartesian_tra_T *switch_expression;
21177 boolean_T b_bool;
21178 int32_T b_kstr;
21179 char_T b[8];
21180 char_T b_0[9];
21181 int32_T loop_ub;
21182 int8_T tmp[6];
21183 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
21184 '1' };
21185
21186 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21187
21188 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21189
21190 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21191 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
21192
21193 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21194 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21195
21196 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21197 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21198 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
21199
21200 int32_T exitg1;
21201 b_obj = obj;
21202 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21203 obj->NameInternal->size[0] = 1;
21204 obj->NameInternal->size[1] = 10;
21205 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
21206 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
21207 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
21208 }
21209
21210 obj->ParentIndex = 1.0;
21211 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21212 obj->JointInternal.Type->size[0] = 1;
21213 obj->JointInternal.Type->size[1] = 8;
21214 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
21215 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21216 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
21217 }
21218
21219 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
21220 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21221 switch_expression->size[0] = 1;
21222 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21223 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
21224 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21225 - 1;
21226 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21227 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21228 }
21229
21230 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21231 b[b_kstr] = tmp_1[b_kstr];
21232 }
21233
21234 b_bool = false;
21235 if (switch_expression->size[1] == 8) {
21236 b_kstr = 1;
21237 do {
21238 exitg1 = 0;
21239 if (b_kstr - 1 < 8) {
21240 loop_ub = b_kstr - 1;
21241 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21242 exitg1 = 1;
21243 } else {
21244 b_kstr++;
21245 }
21246 } else {
21247 b_bool = true;
21248 exitg1 = 1;
21249 }
21250 } while (exitg1 == 0);
21251 }
21252
21253 if (b_bool) {
21254 b_kstr = 0;
21255 } else {
21256 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21257 b_0[b_kstr] = tmp_2[b_kstr];
21258 }
21259
21260 b_bool = false;
21261 if (switch_expression->size[1] == 9) {
21262 b_kstr = 1;
21263 do {
21264 exitg1 = 0;
21265 if (b_kstr - 1 < 9) {
21266 loop_ub = b_kstr - 1;
21267 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21268 exitg1 = 1;
21269 } else {
21270 b_kstr++;
21271 }
21272 } else {
21273 b_bool = true;
21274 exitg1 = 1;
21275 }
21276 } while (exitg1 == 0);
21277 }
21278
21279 if (b_bool) {
21280 b_kstr = 1;
21281 } else {
21282 b_kstr = -1;
21283 }
21284 }
21285
21286 cartesian_traj_emxFree_char_T_a(&switch_expression);
21287 switch (b_kstr) {
21288 case 0:
21289 tmp[0] = 0;
21290 tmp[1] = 0;
21291 tmp[2] = 1;
21292 tmp[3] = 0;
21293 tmp[4] = 0;
21294 tmp[5] = 0;
21295 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21296 msubspace_data[b_kstr] = tmp[b_kstr];
21297 }
21298
21299 obj->JointInternal.PositionNumber = 1.0;
21300 obj->JointInternal.JointAxisInternal[0] = 0.0;
21301 obj->JointInternal.JointAxisInternal[1] = 0.0;
21302 obj->JointInternal.JointAxisInternal[2] = 1.0;
21303 break;
21304
21305 case 1:
21306 tmp[0] = 0;
21307 tmp[1] = 0;
21308 tmp[2] = 0;
21309 tmp[3] = 0;
21310 tmp[4] = 0;
21311 tmp[5] = 1;
21312 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21313 msubspace_data[b_kstr] = tmp[b_kstr];
21314 }
21315
21316 obj->JointInternal.PositionNumber = 1.0;
21317 obj->JointInternal.JointAxisInternal[0] = 0.0;
21318 obj->JointInternal.JointAxisInternal[1] = 0.0;
21319 obj->JointInternal.JointAxisInternal[2] = 1.0;
21320 break;
21321
21322 default:
21323 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21324 msubspace_data[b_kstr] = 0;
21325 }
21326
21327 obj->JointInternal.PositionNumber = 0.0;
21328 obj->JointInternal.JointAxisInternal[0] = 0.0;
21329 obj->JointInternal.JointAxisInternal[1] = 0.0;
21330 obj->JointInternal.JointAxisInternal[2] = 0.0;
21331 break;
21332 }
21333
21334 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21335 obj->JointInternal.MotionSubspace->size[1];
21336 obj->JointInternal.MotionSubspace->size[0] = 6;
21337 obj->JointInternal.MotionSubspace->size[1] = 1;
21338 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21339 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21340 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
21341 }
21342
21343 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21344 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
21345 }
21346
21347 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21348 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
21349 }
21350
21351 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21352 obj->JointInternal.MotionSubspace->size[1];
21353 obj->JointInternal.MotionSubspace->size[0] = 6;
21354 obj->JointInternal.MotionSubspace->size[1] = 1;
21355 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21356 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21357 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
21358 }
21359
21360 obj->JointInternal.JointAxisInternal[0] = 0.0;
21361 obj->JointInternal.JointAxisInternal[1] = 0.0;
21362 obj->JointInternal.JointAxisInternal[2] = 1.0;
21363 return b_obj;
21364}
21365
21366static n_robotics_manip_internal_R_a_T *cartesi_RigidBody_RigidBody_ast
21367 (n_robotics_manip_internal_R_a_T *obj)
21368{
21369 n_robotics_manip_internal_R_a_T *b_obj;
21370 int8_T msubspace_data[36];
21371 emxArray_char_T_cartesian_tra_T *switch_expression;
21372 boolean_T b_bool;
21373 int32_T b_kstr;
21374 char_T b[8];
21375 char_T b_0[9];
21376 int32_T loop_ub;
21377 int8_T tmp[6];
21378 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
21379 '2' };
21380
21381 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21382
21383 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21384
21385 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
21386 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
21387 0.0, 0.0, 0.0, 1.0 };
21388
21389 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21390 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21391
21392 static const real_T tmp_5[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21393 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21394 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
21395
21396 int32_T exitg1;
21397 b_obj = obj;
21398 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21399 obj->NameInternal->size[0] = 1;
21400 obj->NameInternal->size[1] = 10;
21401 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
21402 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
21403 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
21404 }
21405
21406 obj->ParentIndex = 2.0;
21407 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21408 obj->JointInternal.Type->size[0] = 1;
21409 obj->JointInternal.Type->size[1] = 8;
21410 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
21411 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21412 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
21413 }
21414
21415 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
21416 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21417 switch_expression->size[0] = 1;
21418 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21419 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
21420 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21421 - 1;
21422 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21423 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21424 }
21425
21426 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21427 b[b_kstr] = tmp_1[b_kstr];
21428 }
21429
21430 b_bool = false;
21431 if (switch_expression->size[1] == 8) {
21432 b_kstr = 1;
21433 do {
21434 exitg1 = 0;
21435 if (b_kstr - 1 < 8) {
21436 loop_ub = b_kstr - 1;
21437 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21438 exitg1 = 1;
21439 } else {
21440 b_kstr++;
21441 }
21442 } else {
21443 b_bool = true;
21444 exitg1 = 1;
21445 }
21446 } while (exitg1 == 0);
21447 }
21448
21449 if (b_bool) {
21450 b_kstr = 0;
21451 } else {
21452 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21453 b_0[b_kstr] = tmp_2[b_kstr];
21454 }
21455
21456 b_bool = false;
21457 if (switch_expression->size[1] == 9) {
21458 b_kstr = 1;
21459 do {
21460 exitg1 = 0;
21461 if (b_kstr - 1 < 9) {
21462 loop_ub = b_kstr - 1;
21463 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21464 exitg1 = 1;
21465 } else {
21466 b_kstr++;
21467 }
21468 } else {
21469 b_bool = true;
21470 exitg1 = 1;
21471 }
21472 } while (exitg1 == 0);
21473 }
21474
21475 if (b_bool) {
21476 b_kstr = 1;
21477 } else {
21478 b_kstr = -1;
21479 }
21480 }
21481
21482 cartesian_traj_emxFree_char_T_a(&switch_expression);
21483 switch (b_kstr) {
21484 case 0:
21485 tmp[0] = 0;
21486 tmp[1] = 0;
21487 tmp[2] = 1;
21488 tmp[3] = 0;
21489 tmp[4] = 0;
21490 tmp[5] = 0;
21491 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21492 msubspace_data[b_kstr] = tmp[b_kstr];
21493 }
21494
21495 obj->JointInternal.PositionNumber = 1.0;
21496 obj->JointInternal.JointAxisInternal[0] = 0.0;
21497 obj->JointInternal.JointAxisInternal[1] = 0.0;
21498 obj->JointInternal.JointAxisInternal[2] = 1.0;
21499 break;
21500
21501 case 1:
21502 tmp[0] = 0;
21503 tmp[1] = 0;
21504 tmp[2] = 0;
21505 tmp[3] = 0;
21506 tmp[4] = 0;
21507 tmp[5] = 1;
21508 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21509 msubspace_data[b_kstr] = tmp[b_kstr];
21510 }
21511
21512 obj->JointInternal.PositionNumber = 1.0;
21513 obj->JointInternal.JointAxisInternal[0] = 0.0;
21514 obj->JointInternal.JointAxisInternal[1] = 0.0;
21515 obj->JointInternal.JointAxisInternal[2] = 1.0;
21516 break;
21517
21518 default:
21519 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21520 msubspace_data[b_kstr] = 0;
21521 }
21522
21523 obj->JointInternal.PositionNumber = 0.0;
21524 obj->JointInternal.JointAxisInternal[0] = 0.0;
21525 obj->JointInternal.JointAxisInternal[1] = 0.0;
21526 obj->JointInternal.JointAxisInternal[2] = 0.0;
21527 break;
21528 }
21529
21530 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21531 obj->JointInternal.MotionSubspace->size[1];
21532 obj->JointInternal.MotionSubspace->size[0] = 6;
21533 obj->JointInternal.MotionSubspace->size[1] = 1;
21534 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21535 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21536 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
21537 }
21538
21539 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21540 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
21541 }
21542
21543 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21544 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
21545 }
21546
21547 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21548 obj->JointInternal.MotionSubspace->size[1];
21549 obj->JointInternal.MotionSubspace->size[0] = 6;
21550 obj->JointInternal.MotionSubspace->size[1] = 1;
21551 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21552 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21553 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
21554 }
21555
21556 obj->JointInternal.JointAxisInternal[0] = 0.0;
21557 obj->JointInternal.JointAxisInternal[1] = 0.0;
21558 obj->JointInternal.JointAxisInternal[2] = -1.0;
21559 return b_obj;
21560}
21561
21562static n_robotics_manip_internal_R_a_T *cartes_RigidBody_RigidBody_astw
21563 (n_robotics_manip_internal_R_a_T *obj)
21564{
21565 n_robotics_manip_internal_R_a_T *b_obj;
21566 int8_T msubspace_data[36];
21567 emxArray_char_T_cartesian_tra_T *switch_expression;
21568 boolean_T b_bool;
21569 int32_T b_kstr;
21570 char_T b[8];
21571 char_T b_0[9];
21572 int32_T loop_ub;
21573 int8_T tmp[6];
21574 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
21575 '3' };
21576
21577 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21578
21579 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21580
21581 static const real_T tmp_3[16] = { 1.0, 2.0682310711021444E-13,
21582 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
21583 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
21584 1.0 };
21585
21586 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21587 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21588
21589 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21590 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21591 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
21592
21593 int32_T exitg1;
21594 b_obj = obj;
21595 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21596 obj->NameInternal->size[0] = 1;
21597 obj->NameInternal->size[1] = 10;
21598 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
21599 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
21600 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
21601 }
21602
21603 obj->ParentIndex = 3.0;
21604 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21605 obj->JointInternal.Type->size[0] = 1;
21606 obj->JointInternal.Type->size[1] = 8;
21607 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
21608 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21609 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
21610 }
21611
21612 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
21613 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21614 switch_expression->size[0] = 1;
21615 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21616 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
21617 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21618 - 1;
21619 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21620 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21621 }
21622
21623 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21624 b[b_kstr] = tmp_1[b_kstr];
21625 }
21626
21627 b_bool = false;
21628 if (switch_expression->size[1] == 8) {
21629 b_kstr = 1;
21630 do {
21631 exitg1 = 0;
21632 if (b_kstr - 1 < 8) {
21633 loop_ub = b_kstr - 1;
21634 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21635 exitg1 = 1;
21636 } else {
21637 b_kstr++;
21638 }
21639 } else {
21640 b_bool = true;
21641 exitg1 = 1;
21642 }
21643 } while (exitg1 == 0);
21644 }
21645
21646 if (b_bool) {
21647 b_kstr = 0;
21648 } else {
21649 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21650 b_0[b_kstr] = tmp_2[b_kstr];
21651 }
21652
21653 b_bool = false;
21654 if (switch_expression->size[1] == 9) {
21655 b_kstr = 1;
21656 do {
21657 exitg1 = 0;
21658 if (b_kstr - 1 < 9) {
21659 loop_ub = b_kstr - 1;
21660 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21661 exitg1 = 1;
21662 } else {
21663 b_kstr++;
21664 }
21665 } else {
21666 b_bool = true;
21667 exitg1 = 1;
21668 }
21669 } while (exitg1 == 0);
21670 }
21671
21672 if (b_bool) {
21673 b_kstr = 1;
21674 } else {
21675 b_kstr = -1;
21676 }
21677 }
21678
21679 cartesian_traj_emxFree_char_T_a(&switch_expression);
21680 switch (b_kstr) {
21681 case 0:
21682 tmp[0] = 0;
21683 tmp[1] = 0;
21684 tmp[2] = 1;
21685 tmp[3] = 0;
21686 tmp[4] = 0;
21687 tmp[5] = 0;
21688 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21689 msubspace_data[b_kstr] = tmp[b_kstr];
21690 }
21691
21692 obj->JointInternal.PositionNumber = 1.0;
21693 obj->JointInternal.JointAxisInternal[0] = 0.0;
21694 obj->JointInternal.JointAxisInternal[1] = 0.0;
21695 obj->JointInternal.JointAxisInternal[2] = 1.0;
21696 break;
21697
21698 case 1:
21699 tmp[0] = 0;
21700 tmp[1] = 0;
21701 tmp[2] = 0;
21702 tmp[3] = 0;
21703 tmp[4] = 0;
21704 tmp[5] = 1;
21705 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21706 msubspace_data[b_kstr] = tmp[b_kstr];
21707 }
21708
21709 obj->JointInternal.PositionNumber = 1.0;
21710 obj->JointInternal.JointAxisInternal[0] = 0.0;
21711 obj->JointInternal.JointAxisInternal[1] = 0.0;
21712 obj->JointInternal.JointAxisInternal[2] = 1.0;
21713 break;
21714
21715 default:
21716 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21717 msubspace_data[b_kstr] = 0;
21718 }
21719
21720 obj->JointInternal.PositionNumber = 0.0;
21721 obj->JointInternal.JointAxisInternal[0] = 0.0;
21722 obj->JointInternal.JointAxisInternal[1] = 0.0;
21723 obj->JointInternal.JointAxisInternal[2] = 0.0;
21724 break;
21725 }
21726
21727 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21728 obj->JointInternal.MotionSubspace->size[1];
21729 obj->JointInternal.MotionSubspace->size[0] = 6;
21730 obj->JointInternal.MotionSubspace->size[1] = 1;
21731 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21732 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21733 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
21734 }
21735
21736 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21737 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
21738 }
21739
21740 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21741 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
21742 }
21743
21744 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21745 obj->JointInternal.MotionSubspace->size[1];
21746 obj->JointInternal.MotionSubspace->size[0] = 6;
21747 obj->JointInternal.MotionSubspace->size[1] = 1;
21748 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21749 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21750 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
21751 }
21752
21753 obj->JointInternal.JointAxisInternal[0] = 0.0;
21754 obj->JointInternal.JointAxisInternal[1] = 0.0;
21755 obj->JointInternal.JointAxisInternal[2] = 1.0;
21756 return b_obj;
21757}
21758
21759static n_robotics_manip_internal_R_a_T *carte_RigidBody_RigidBody_astwh
21760 (n_robotics_manip_internal_R_a_T *obj)
21761{
21762 n_robotics_manip_internal_R_a_T *b_obj;
21763 int8_T msubspace_data[36];
21764 emxArray_char_T_cartesian_tra_T *switch_expression;
21765 boolean_T b_bool;
21766 int32_T b_kstr;
21767 char_T b[8];
21768 char_T b_0[9];
21769 int32_T loop_ub;
21770 int8_T tmp[6];
21771 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
21772 '4' };
21773
21774 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21775
21776 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21777
21778 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
21779 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
21780 0.0, -0.268, 0.0, 1.0 };
21781
21782 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21783 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21784
21785 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21786 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21787 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
21788
21789 int32_T exitg1;
21790 b_obj = obj;
21791 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21792 obj->NameInternal->size[0] = 1;
21793 obj->NameInternal->size[1] = 10;
21794 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
21795 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
21796 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
21797 }
21798
21799 obj->ParentIndex = 4.0;
21800 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21801 obj->JointInternal.Type->size[0] = 1;
21802 obj->JointInternal.Type->size[1] = 8;
21803 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
21804 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21805 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
21806 }
21807
21808 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
21809 b_kstr = switch_expression->size[0] * switch_expression->size[1];
21810 switch_expression->size[0] = 1;
21811 switch_expression->size[1] = obj->JointInternal.Type->size[1];
21812 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
21813 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
21814 - 1;
21815 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
21816 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
21817 }
21818
21819 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
21820 b[b_kstr] = tmp_1[b_kstr];
21821 }
21822
21823 b_bool = false;
21824 if (switch_expression->size[1] == 8) {
21825 b_kstr = 1;
21826 do {
21827 exitg1 = 0;
21828 if (b_kstr - 1 < 8) {
21829 loop_ub = b_kstr - 1;
21830 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21831 exitg1 = 1;
21832 } else {
21833 b_kstr++;
21834 }
21835 } else {
21836 b_bool = true;
21837 exitg1 = 1;
21838 }
21839 } while (exitg1 == 0);
21840 }
21841
21842 if (b_bool) {
21843 b_kstr = 0;
21844 } else {
21845 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21846 b_0[b_kstr] = tmp_2[b_kstr];
21847 }
21848
21849 b_bool = false;
21850 if (switch_expression->size[1] == 9) {
21851 b_kstr = 1;
21852 do {
21853 exitg1 = 0;
21854 if (b_kstr - 1 < 9) {
21855 loop_ub = b_kstr - 1;
21856 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21857 exitg1 = 1;
21858 } else {
21859 b_kstr++;
21860 }
21861 } else {
21862 b_bool = true;
21863 exitg1 = 1;
21864 }
21865 } while (exitg1 == 0);
21866 }
21867
21868 if (b_bool) {
21869 b_kstr = 1;
21870 } else {
21871 b_kstr = -1;
21872 }
21873 }
21874
21875 cartesian_traj_emxFree_char_T_a(&switch_expression);
21876 switch (b_kstr) {
21877 case 0:
21878 tmp[0] = 0;
21879 tmp[1] = 0;
21880 tmp[2] = 1;
21881 tmp[3] = 0;
21882 tmp[4] = 0;
21883 tmp[5] = 0;
21884 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21885 msubspace_data[b_kstr] = tmp[b_kstr];
21886 }
21887
21888 obj->JointInternal.PositionNumber = 1.0;
21889 obj->JointInternal.JointAxisInternal[0] = 0.0;
21890 obj->JointInternal.JointAxisInternal[1] = 0.0;
21891 obj->JointInternal.JointAxisInternal[2] = 1.0;
21892 break;
21893
21894 case 1:
21895 tmp[0] = 0;
21896 tmp[1] = 0;
21897 tmp[2] = 0;
21898 tmp[3] = 0;
21899 tmp[4] = 0;
21900 tmp[5] = 1;
21901 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21902 msubspace_data[b_kstr] = tmp[b_kstr];
21903 }
21904
21905 obj->JointInternal.PositionNumber = 1.0;
21906 obj->JointInternal.JointAxisInternal[0] = 0.0;
21907 obj->JointInternal.JointAxisInternal[1] = 0.0;
21908 obj->JointInternal.JointAxisInternal[2] = 1.0;
21909 break;
21910
21911 default:
21912 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21913 msubspace_data[b_kstr] = 0;
21914 }
21915
21916 obj->JointInternal.PositionNumber = 0.0;
21917 obj->JointInternal.JointAxisInternal[0] = 0.0;
21918 obj->JointInternal.JointAxisInternal[1] = 0.0;
21919 obj->JointInternal.JointAxisInternal[2] = 0.0;
21920 break;
21921 }
21922
21923 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21924 obj->JointInternal.MotionSubspace->size[1];
21925 obj->JointInternal.MotionSubspace->size[0] = 6;
21926 obj->JointInternal.MotionSubspace->size[1] = 1;
21927 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21928 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21929 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
21930 }
21931
21932 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21933 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
21934 }
21935
21936 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
21937 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
21938 }
21939
21940 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21941 obj->JointInternal.MotionSubspace->size[1];
21942 obj->JointInternal.MotionSubspace->size[0] = 6;
21943 obj->JointInternal.MotionSubspace->size[1] = 1;
21944 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21945 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21946 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
21947 }
21948
21949 obj->JointInternal.JointAxisInternal[0] = 0.0;
21950 obj->JointInternal.JointAxisInternal[1] = 0.0;
21951 obj->JointInternal.JointAxisInternal[2] = 1.0;
21952 return b_obj;
21953}
21954
21955static n_robotics_manip_internal_R_a_T *cart_RigidBody_RigidBody_astwhq
21956 (n_robotics_manip_internal_R_a_T *obj)
21957{
21958 n_robotics_manip_internal_R_a_T *b_obj;
21959 int8_T msubspace_data[36];
21960 emxArray_char_T_cartesian_tra_T *switch_expression;
21961 boolean_T b_bool;
21962 int32_T b_kstr;
21963 char_T b[8];
21964 char_T b_0[9];
21965 int32_T loop_ub;
21966 int8_T tmp[6];
21967 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
21968 '5' };
21969
21970 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21971
21972 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21973
21974 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21975 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21976
21977 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
21978 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
21979
21980 static const real_T tmp_5[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21981 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
21982 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
21983
21984 int32_T exitg1;
21985 b_obj = obj;
21986 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
21987 obj->NameInternal->size[0] = 1;
21988 obj->NameInternal->size[1] = 10;
21989 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
21990 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
21991 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
21992 }
21993
21994 obj->ParentIndex = 5.0;
21995 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
21996 obj->JointInternal.Type->size[0] = 1;
21997 obj->JointInternal.Type->size[1] = 8;
21998 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
21999 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22000 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
22001 }
22002
22003 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
22004 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22005 switch_expression->size[0] = 1;
22006 switch_expression->size[1] = obj->JointInternal.Type->size[1];
22007 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
22008 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
22009 - 1;
22010 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22011 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
22012 }
22013
22014 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22015 b[b_kstr] = tmp_1[b_kstr];
22016 }
22017
22018 b_bool = false;
22019 if (switch_expression->size[1] == 8) {
22020 b_kstr = 1;
22021 do {
22022 exitg1 = 0;
22023 if (b_kstr - 1 < 8) {
22024 loop_ub = b_kstr - 1;
22025 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22026 exitg1 = 1;
22027 } else {
22028 b_kstr++;
22029 }
22030 } else {
22031 b_bool = true;
22032 exitg1 = 1;
22033 }
22034 } while (exitg1 == 0);
22035 }
22036
22037 if (b_bool) {
22038 b_kstr = 0;
22039 } else {
22040 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22041 b_0[b_kstr] = tmp_2[b_kstr];
22042 }
22043
22044 b_bool = false;
22045 if (switch_expression->size[1] == 9) {
22046 b_kstr = 1;
22047 do {
22048 exitg1 = 0;
22049 if (b_kstr - 1 < 9) {
22050 loop_ub = b_kstr - 1;
22051 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22052 exitg1 = 1;
22053 } else {
22054 b_kstr++;
22055 }
22056 } else {
22057 b_bool = true;
22058 exitg1 = 1;
22059 }
22060 } while (exitg1 == 0);
22061 }
22062
22063 if (b_bool) {
22064 b_kstr = 1;
22065 } else {
22066 b_kstr = -1;
22067 }
22068 }
22069
22070 cartesian_traj_emxFree_char_T_a(&switch_expression);
22071 switch (b_kstr) {
22072 case 0:
22073 tmp[0] = 0;
22074 tmp[1] = 0;
22075 tmp[2] = 1;
22076 tmp[3] = 0;
22077 tmp[4] = 0;
22078 tmp[5] = 0;
22079 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22080 msubspace_data[b_kstr] = tmp[b_kstr];
22081 }
22082
22083 obj->JointInternal.PositionNumber = 1.0;
22084 obj->JointInternal.JointAxisInternal[0] = 0.0;
22085 obj->JointInternal.JointAxisInternal[1] = 0.0;
22086 obj->JointInternal.JointAxisInternal[2] = 1.0;
22087 break;
22088
22089 case 1:
22090 tmp[0] = 0;
22091 tmp[1] = 0;
22092 tmp[2] = 0;
22093 tmp[3] = 0;
22094 tmp[4] = 0;
22095 tmp[5] = 1;
22096 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22097 msubspace_data[b_kstr] = tmp[b_kstr];
22098 }
22099
22100 obj->JointInternal.PositionNumber = 1.0;
22101 obj->JointInternal.JointAxisInternal[0] = 0.0;
22102 obj->JointInternal.JointAxisInternal[1] = 0.0;
22103 obj->JointInternal.JointAxisInternal[2] = 1.0;
22104 break;
22105
22106 default:
22107 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22108 msubspace_data[b_kstr] = 0;
22109 }
22110
22111 obj->JointInternal.PositionNumber = 0.0;
22112 obj->JointInternal.JointAxisInternal[0] = 0.0;
22113 obj->JointInternal.JointAxisInternal[1] = 0.0;
22114 obj->JointInternal.JointAxisInternal[2] = 0.0;
22115 break;
22116 }
22117
22118 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
22119 obj->JointInternal.MotionSubspace->size[1];
22120 obj->JointInternal.MotionSubspace->size[0] = 6;
22121 obj->JointInternal.MotionSubspace->size[1] = 1;
22122 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
22123 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22124 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
22125 }
22126
22127 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22128 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
22129 }
22130
22131 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22132 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
22133 }
22134
22135 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
22136 obj->JointInternal.MotionSubspace->size[1];
22137 obj->JointInternal.MotionSubspace->size[0] = 6;
22138 obj->JointInternal.MotionSubspace->size[1] = 1;
22139 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
22140 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22141 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
22142 }
22143
22144 obj->JointInternal.JointAxisInternal[0] = 0.0;
22145 obj->JointInternal.JointAxisInternal[1] = 1.0;
22146 obj->JointInternal.JointAxisInternal[2] = 0.0;
22147 return b_obj;
22148}
22149
22150static n_robotics_manip_internal_R_a_T *car_RigidBody_RigidBody_astwhqf
22151 (n_robotics_manip_internal_R_a_T *obj)
22152{
22153 n_robotics_manip_internal_R_a_T *b_obj;
22154 int8_T msubspace_data[36];
22155 emxArray_char_T_cartesian_tra_T *switch_expression;
22156 boolean_T b_bool;
22157 int32_T b_kstr;
22158 char_T b[8];
22159 char_T b_0[9];
22160 int32_T loop_ub;
22161 int8_T tmp[6];
22162 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
22163 '6' };
22164
22165 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
22166
22167 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
22168
22169 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22170 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
22171
22172 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22173 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22174
22175 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
22176 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
22177 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
22178
22179 int32_T exitg1;
22180 b_obj = obj;
22181 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
22182 obj->NameInternal->size[0] = 1;
22183 obj->NameInternal->size[1] = 10;
22184 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
22185 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
22186 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
22187 }
22188
22189 obj->ParentIndex = 6.0;
22190 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
22191 obj->JointInternal.Type->size[0] = 1;
22192 obj->JointInternal.Type->size[1] = 8;
22193 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
22194 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22195 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
22196 }
22197
22198 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
22199 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22200 switch_expression->size[0] = 1;
22201 switch_expression->size[1] = obj->JointInternal.Type->size[1];
22202 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
22203 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
22204 - 1;
22205 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22206 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
22207 }
22208
22209 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22210 b[b_kstr] = tmp_1[b_kstr];
22211 }
22212
22213 b_bool = false;
22214 if (switch_expression->size[1] == 8) {
22215 b_kstr = 1;
22216 do {
22217 exitg1 = 0;
22218 if (b_kstr - 1 < 8) {
22219 loop_ub = b_kstr - 1;
22220 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22221 exitg1 = 1;
22222 } else {
22223 b_kstr++;
22224 }
22225 } else {
22226 b_bool = true;
22227 exitg1 = 1;
22228 }
22229 } while (exitg1 == 0);
22230 }
22231
22232 if (b_bool) {
22233 b_kstr = 0;
22234 } else {
22235 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22236 b_0[b_kstr] = tmp_2[b_kstr];
22237 }
22238
22239 b_bool = false;
22240 if (switch_expression->size[1] == 9) {
22241 b_kstr = 1;
22242 do {
22243 exitg1 = 0;
22244 if (b_kstr - 1 < 9) {
22245 loop_ub = b_kstr - 1;
22246 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22247 exitg1 = 1;
22248 } else {
22249 b_kstr++;
22250 }
22251 } else {
22252 b_bool = true;
22253 exitg1 = 1;
22254 }
22255 } while (exitg1 == 0);
22256 }
22257
22258 if (b_bool) {
22259 b_kstr = 1;
22260 } else {
22261 b_kstr = -1;
22262 }
22263 }
22264
22265 cartesian_traj_emxFree_char_T_a(&switch_expression);
22266 switch (b_kstr) {
22267 case 0:
22268 tmp[0] = 0;
22269 tmp[1] = 0;
22270 tmp[2] = 1;
22271 tmp[3] = 0;
22272 tmp[4] = 0;
22273 tmp[5] = 0;
22274 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22275 msubspace_data[b_kstr] = tmp[b_kstr];
22276 }
22277
22278 obj->JointInternal.PositionNumber = 1.0;
22279 obj->JointInternal.JointAxisInternal[0] = 0.0;
22280 obj->JointInternal.JointAxisInternal[1] = 0.0;
22281 obj->JointInternal.JointAxisInternal[2] = 1.0;
22282 break;
22283
22284 case 1:
22285 tmp[0] = 0;
22286 tmp[1] = 0;
22287 tmp[2] = 0;
22288 tmp[3] = 0;
22289 tmp[4] = 0;
22290 tmp[5] = 1;
22291 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22292 msubspace_data[b_kstr] = tmp[b_kstr];
22293 }
22294
22295 obj->JointInternal.PositionNumber = 1.0;
22296 obj->JointInternal.JointAxisInternal[0] = 0.0;
22297 obj->JointInternal.JointAxisInternal[1] = 0.0;
22298 obj->JointInternal.JointAxisInternal[2] = 1.0;
22299 break;
22300
22301 default:
22302 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22303 msubspace_data[b_kstr] = 0;
22304 }
22305
22306 obj->JointInternal.PositionNumber = 0.0;
22307 obj->JointInternal.JointAxisInternal[0] = 0.0;
22308 obj->JointInternal.JointAxisInternal[1] = 0.0;
22309 obj->JointInternal.JointAxisInternal[2] = 0.0;
22310 break;
22311 }
22312
22313 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
22314 obj->JointInternal.MotionSubspace->size[1];
22315 obj->JointInternal.MotionSubspace->size[0] = 6;
22316 obj->JointInternal.MotionSubspace->size[1] = 1;
22317 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
22318 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22319 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
22320 }
22321
22322 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22323 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
22324 }
22325
22326 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22327 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
22328 }
22329
22330 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
22331 obj->JointInternal.MotionSubspace->size[1];
22332 obj->JointInternal.MotionSubspace->size[0] = 6;
22333 obj->JointInternal.MotionSubspace->size[1] = 1;
22334 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
22335 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22336 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
22337 }
22338
22339 obj->JointInternal.JointAxisInternal[0] = 0.0;
22340 obj->JointInternal.JointAxisInternal[1] = 0.0;
22341 obj->JointInternal.JointAxisInternal[2] = 1.0;
22342 return b_obj;
22343}
22344
22345static n_robotics_manip_internal_R_a_T *ca_RigidBody_RigidBody_astwhqf2
22346 (n_robotics_manip_internal_R_a_T *obj)
22347{
22348 n_robotics_manip_internal_R_a_T *b_obj;
22349 int8_T msubspace_data[36];
22350 emxArray_char_T_cartesian_tra_T *switch_expression;
22351 boolean_T b_bool;
22352 int32_T b_kstr;
22353 char_T b[8];
22354 char_T b_0[9];
22355 int32_T loop_ub;
22356 int8_T tmp[6];
22357 static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
22358 'e', 'e' };
22359
22360 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
22361
22362 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
22363
22364 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
22365
22366 static const real_T tmp_4[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22367 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22368
22369 static const real_T tmp_5[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22370 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22371
22372 int32_T exitg1;
22373 b_obj = obj;
22374 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
22375 obj->NameInternal->size[0] = 1;
22376 obj->NameInternal->size[1] = 11;
22377 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
22378 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
22379 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
22380 }
22381
22382 obj->ParentIndex = 7.0;
22383 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
22384 obj->JointInternal.Type->size[0] = 1;
22385 obj->JointInternal.Type->size[1] = 5;
22386 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
22387 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
22388 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
22389 }
22390
22391 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
22392 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22393 switch_expression->size[0] = 1;
22394 switch_expression->size[1] = obj->JointInternal.Type->size[1];
22395 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
22396 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
22397 - 1;
22398 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22399 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
22400 }
22401
22402 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22403 b[b_kstr] = tmp_2[b_kstr];
22404 }
22405
22406 b_bool = false;
22407 if (switch_expression->size[1] == 8) {
22408 b_kstr = 1;
22409 do {
22410 exitg1 = 0;
22411 if (b_kstr - 1 < 8) {
22412 loop_ub = b_kstr - 1;
22413 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22414 exitg1 = 1;
22415 } else {
22416 b_kstr++;
22417 }
22418 } else {
22419 b_bool = true;
22420 exitg1 = 1;
22421 }
22422 } while (exitg1 == 0);
22423 }
22424
22425 if (b_bool) {
22426 b_kstr = 0;
22427 } else {
22428 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22429 b_0[b_kstr] = tmp_3[b_kstr];
22430 }
22431
22432 b_bool = false;
22433 if (switch_expression->size[1] == 9) {
22434 b_kstr = 1;
22435 do {
22436 exitg1 = 0;
22437 if (b_kstr - 1 < 9) {
22438 loop_ub = b_kstr - 1;
22439 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22440 exitg1 = 1;
22441 } else {
22442 b_kstr++;
22443 }
22444 } else {
22445 b_bool = true;
22446 exitg1 = 1;
22447 }
22448 } while (exitg1 == 0);
22449 }
22450
22451 if (b_bool) {
22452 b_kstr = 1;
22453 } else {
22454 b_kstr = -1;
22455 }
22456 }
22457
22458 cartesian_traj_emxFree_char_T_a(&switch_expression);
22459 switch (b_kstr) {
22460 case 0:
22461 tmp[0] = 0;
22462 tmp[1] = 0;
22463 tmp[2] = 1;
22464 tmp[3] = 0;
22465 tmp[4] = 0;
22466 tmp[5] = 0;
22467 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22468 msubspace_data[b_kstr] = tmp[b_kstr];
22469 }
22470
22471 obj->JointInternal.PositionNumber = 1.0;
22472 obj->JointInternal.JointAxisInternal[0] = 0.0;
22473 obj->JointInternal.JointAxisInternal[1] = 0.0;
22474 obj->JointInternal.JointAxisInternal[2] = 1.0;
22475 break;
22476
22477 case 1:
22478 tmp[0] = 0;
22479 tmp[1] = 0;
22480 tmp[2] = 0;
22481 tmp[3] = 0;
22482 tmp[4] = 0;
22483 tmp[5] = 1;
22484 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22485 msubspace_data[b_kstr] = tmp[b_kstr];
22486 }
22487
22488 obj->JointInternal.PositionNumber = 1.0;
22489 obj->JointInternal.JointAxisInternal[0] = 0.0;
22490 obj->JointInternal.JointAxisInternal[1] = 0.0;
22491 obj->JointInternal.JointAxisInternal[2] = 1.0;
22492 break;
22493
22494 default:
22495 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22496 msubspace_data[b_kstr] = 0;
22497 }
22498
22499 obj->JointInternal.PositionNumber = 0.0;
22500 obj->JointInternal.JointAxisInternal[0] = 0.0;
22501 obj->JointInternal.JointAxisInternal[1] = 0.0;
22502 obj->JointInternal.JointAxisInternal[2] = 0.0;
22503 break;
22504 }
22505
22506 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
22507 obj->JointInternal.MotionSubspace->size[1];
22508 obj->JointInternal.MotionSubspace->size[0] = 6;
22509 obj->JointInternal.MotionSubspace->size[1] = 1;
22510 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
22511 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22512 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
22513 }
22514
22515 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22516 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_4[b_kstr];
22517 }
22518
22519 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22520 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_5[b_kstr];
22521 }
22522
22523 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
22524 obj->JointInternal.MotionSubspace->size[1];
22525 obj->JointInternal.MotionSubspace->size[0] = 6;
22526 obj->JointInternal.MotionSubspace->size[1] = 1;
22527 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
22528 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22529 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
22530 }
22531
22532 obj->JointInternal.JointAxisInternal[0] = 0.0;
22533 obj->JointInternal.JointAxisInternal[1] = 0.0;
22534 obj->JointInternal.JointAxisInternal[2] = 0.0;
22535 return b_obj;
22536}
22537
22538static o_robotics_manip_internal_R_a_T *c_RigidBody_RigidBody_astwhqf2a
22539 (o_robotics_manip_internal_R_a_T *obj)
22540{
22541 o_robotics_manip_internal_R_a_T *b_obj;
22542 int8_T msubspace_data[36];
22543 emxArray_char_T_cartesian_tra_T *switch_expression;
22544 boolean_T b_bool;
22545 int32_T b_kstr;
22546 char_T b[8];
22547 char_T b_0[9];
22548 int32_T loop_ub;
22549 int8_T tmp[6];
22550 static const char_T tmp_0[5] = { 'w', 'o', 'r', 'l', 'd' };
22551
22552 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
22553
22554 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
22555
22556 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
22557
22558 int32_T exitg1;
22559 b_obj = obj;
22560 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
22561 obj->NameInternal->size[0] = 1;
22562 obj->NameInternal->size[1] = 5;
22563 cart_emxEnsureCapacity_char_T_a(obj->NameInternal, b_kstr);
22564 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
22565 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
22566 }
22567
22568 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
22569 obj->JointInternal.Type->size[0] = 1;
22570 obj->JointInternal.Type->size[1] = 5;
22571 cart_emxEnsureCapacity_char_T_a(obj->JointInternal.Type, b_kstr);
22572 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
22573 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
22574 }
22575
22576 cartesian_traj_emxInit_char_T_a(&switch_expression, 2);
22577 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22578 switch_expression->size[0] = 1;
22579 switch_expression->size[1] = obj->JointInternal.Type->size[1];
22580 cart_emxEnsureCapacity_char_T_a(switch_expression, b_kstr);
22581 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
22582 - 1;
22583 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22584 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
22585 }
22586
22587 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22588 b[b_kstr] = tmp_2[b_kstr];
22589 }
22590
22591 b_bool = false;
22592 if (switch_expression->size[1] == 8) {
22593 b_kstr = 1;
22594 do {
22595 exitg1 = 0;
22596 if (b_kstr - 1 < 8) {
22597 loop_ub = b_kstr - 1;
22598 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22599 exitg1 = 1;
22600 } else {
22601 b_kstr++;
22602 }
22603 } else {
22604 b_bool = true;
22605 exitg1 = 1;
22606 }
22607 } while (exitg1 == 0);
22608 }
22609
22610 if (b_bool) {
22611 b_kstr = 0;
22612 } else {
22613 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22614 b_0[b_kstr] = tmp_3[b_kstr];
22615 }
22616
22617 b_bool = false;
22618 if (switch_expression->size[1] == 9) {
22619 b_kstr = 1;
22620 do {
22621 exitg1 = 0;
22622 if (b_kstr - 1 < 9) {
22623 loop_ub = b_kstr - 1;
22624 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22625 exitg1 = 1;
22626 } else {
22627 b_kstr++;
22628 }
22629 } else {
22630 b_bool = true;
22631 exitg1 = 1;
22632 }
22633 } while (exitg1 == 0);
22634 }
22635
22636 if (b_bool) {
22637 b_kstr = 1;
22638 } else {
22639 b_kstr = -1;
22640 }
22641 }
22642
22643 cartesian_traj_emxFree_char_T_a(&switch_expression);
22644 switch (b_kstr) {
22645 case 0:
22646 tmp[0] = 0;
22647 tmp[1] = 0;
22648 tmp[2] = 1;
22649 tmp[3] = 0;
22650 tmp[4] = 0;
22651 tmp[5] = 0;
22652 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22653 msubspace_data[b_kstr] = tmp[b_kstr];
22654 }
22655
22656 obj->JointInternal.PositionNumber = 1.0;
22657 obj->JointInternal.JointAxisInternal[0] = 0.0;
22658 obj->JointInternal.JointAxisInternal[1] = 0.0;
22659 obj->JointInternal.JointAxisInternal[2] = 1.0;
22660 break;
22661
22662 case 1:
22663 tmp[0] = 0;
22664 tmp[1] = 0;
22665 tmp[2] = 0;
22666 tmp[3] = 0;
22667 tmp[4] = 0;
22668 tmp[5] = 1;
22669 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22670 msubspace_data[b_kstr] = tmp[b_kstr];
22671 }
22672
22673 obj->JointInternal.PositionNumber = 1.0;
22674 obj->JointInternal.JointAxisInternal[0] = 0.0;
22675 obj->JointInternal.JointAxisInternal[1] = 0.0;
22676 obj->JointInternal.JointAxisInternal[2] = 1.0;
22677 break;
22678
22679 default:
22680 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22681 msubspace_data[b_kstr] = 0;
22682 }
22683
22684 obj->JointInternal.PositionNumber = 0.0;
22685 obj->JointInternal.JointAxisInternal[0] = 0.0;
22686 obj->JointInternal.JointAxisInternal[1] = 0.0;
22687 obj->JointInternal.JointAxisInternal[2] = 0.0;
22688 break;
22689 }
22690
22691 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
22692 obj->JointInternal.MotionSubspace->size[1];
22693 obj->JointInternal.MotionSubspace->size[0] = 6;
22694 obj->JointInternal.MotionSubspace->size[1] = 1;
22695 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
22696 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
22697 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
22698 }
22699
22700 return b_obj;
22701}
22702
22703static p_robotics_manip_internal_R_a_T *c_RigidBodyTree_RigidBodyTree_a
22704 (p_robotics_manip_internal_R_a_T *obj, n_robotics_manip_internal_R_a_T *iobj_0,
22705 n_robotics_manip_internal_R_a_T *iobj_1, n_robotics_manip_internal_R_a_T
22706 *iobj_2, n_robotics_manip_internal_R_a_T *iobj_3,
22707 n_robotics_manip_internal_R_a_T *iobj_4, n_robotics_manip_internal_R_a_T
22708 *iobj_5, n_robotics_manip_internal_R_a_T *iobj_6,
22709 n_robotics_manip_internal_R_a_T *iobj_7)
22710{
22711 p_robotics_manip_internal_R_a_T *b_obj;
22712 int32_T i;
22713 static const int8_T tmp[16] = { 0, 1, 2, 3, 4, 5, 6, 0, -1, 1, 2, 3, 4, 5, 6,
22714 -1 };
22715
22716 b_obj = obj;
22717 obj->Bodies[0] = cartesian_RigidBody_RigidBody_a(iobj_0);
22718 obj->Bodies[0]->Index = 1.0;
22719 obj->Bodies[1] = cartesia_RigidBody_RigidBody_as(iobj_7);
22720 obj->Bodies[1]->Index = 2.0;
22721 obj->Bodies[2] = cartesi_RigidBody_RigidBody_ast(iobj_1);
22722 obj->Bodies[2]->Index = 3.0;
22723 obj->Bodies[3] = cartes_RigidBody_RigidBody_astw(iobj_2);
22724 obj->Bodies[3]->Index = 4.0;
22725 obj->Bodies[4] = carte_RigidBody_RigidBody_astwh(iobj_3);
22726 obj->Bodies[4]->Index = 5.0;
22727 obj->Bodies[5] = cart_RigidBody_RigidBody_astwhq(iobj_4);
22728 obj->Bodies[5]->Index = 6.0;
22729 obj->Bodies[6] = car_RigidBody_RigidBody_astwhqf(iobj_5);
22730 obj->Bodies[6]->Index = 7.0;
22731 obj->Bodies[7] = ca_RigidBody_RigidBody_astwhqf2(iobj_6);
22732 obj->Bodies[7]->Index = 8.0;
22733 obj->NumBodies = 8.0;
22734 obj->PositionNumber = 6.0;
22735 obj->VelocityNumber = 6.0;
22736 for (i = 0; i < 16; i++) {
22737 obj->PositionDoFMap[i] = tmp[i];
22738 }
22739
22740 c_RigidBody_RigidBody_astwhqf2a(&obj->Base);
22741 return b_obj;
22742}
22743
22744// Model step function
22745void cartesian_trajectory_planner_step(void)
22746{
22747 if (rtmIsMajorTimeStep(cartesian_trajectory_planner_M)) {
22748 // set solver stop time
22749 rtsiSetSolverStopTime(&cartesian_trajectory_planner_M->solverInfo,
22750 ((cartesian_trajectory_planner_M->Timing.clockTick0+1)*
22751 cartesian_trajectory_planner_M->Timing.stepSize0));
22752 } // end MajorTimeStep
22753
22754 // Update absolute time of base rate at minor time step
22755 if (rtmIsMinorTimeStep(cartesian_trajectory_planner_M)) {
22756 cartesian_trajectory_planner_M->Timing.t[0] = rtsiGetT
22757 (&cartesian_trajectory_planner_M->solverInfo);
22758 }
22759
22760 {
22761 robotics_slmanip_internal__as_T *obj;
22762 emxArray_real_T_cartesian_tra_T *b;
22763 if (rtmIsMajorTimeStep(cartesian_trajectory_planner_M)) {
22764 // Outputs for Atomic SubSystem: '<Root>/Subscribe'
22765 // MATLABSystem: '<S10>/SourceBlock' incorporates:
22766 // Inport: '<S14>/In1'
22767
22768 cartesian_trajectory_planner_B.b_varargout_1 =
22769 Sub_cartesian_trajectory_planner_280.getLatestMessage
22770 (&cartesian_trajectory_planner_B.b_varargout_2);
22771
22772 // Outputs for Enabled SubSystem: '<S10>/Enabled Subsystem' incorporates:
22773 // EnablePort: '<S14>/Enable'
22774
22775 if (cartesian_trajectory_planner_B.b_varargout_1) {
22776 cartesian_trajectory_planner_B.In1 =
22777 cartesian_trajectory_planner_B.b_varargout_2;
22778 }
22779
22780 // End of MATLABSystem: '<S10>/SourceBlock'
22781 // End of Outputs for SubSystem: '<S10>/Enabled Subsystem'
22782 // End of Outputs for SubSystem: '<Root>/Subscribe'
22783
22784 // MATLAB Function: '<Root>/MATLAB Function1'
22785 cartesian_trajectory_planner_B.time =
22786 cartesian_trajectory_planner_B.In1.Clock_.Nsec / 1.0E+9 +
22787 cartesian_trajectory_planner_B.In1.Clock_.Sec;
22788 }
22789
22790 // MATLABSystem: '<Root>/Get Parameter'
22791 ParamGet_cartesian_trajectory_planner_271.get_parameter
22792 (&cartesian_trajectory_planner_B.rtb_signal2_idx_0);
22793
22794 // MATLABSystem: '<Root>/Get Parameter1'
22795 ParamGet_cartesian_trajectory_planner_272.get_parameter
22796 (&cartesian_trajectory_planner_B.rtb_signal2_idx_1);
22797
22798 // MATLABSystem: '<S15>/Get Parameter3'
22799 ParamGet_cartesian_trajectory_planner_291.get_parameter
22800 (&cartesian_trajectory_planner_B.tc);
22801
22802 // MATLABSystem: '<S15>/Get Parameter4'
22803 ParamGet_cartesian_trajectory_planner_292.get_parameter
22804 (&cartesian_trajectory_planner_B.SpMax);
22805
22806 // MATLABSystem: '<S15>/Get Parameter5'
22807 ParamGet_cartesian_trajectory_planner_293.get_parameter
22808 (&cartesian_trajectory_planner_B.maxval);
22809
22810 // MATLABSystem: '<S15>/Get Parameter6'
22811 ParamGet_cartesian_trajectory_planner_294.get_parameter
22812 (&cartesian_trajectory_planner_B.s_m);
22813
22814 // MATLABSystem: '<S15>/Get Parameter'
22815 ParamGet_cartesian_trajectory_planner_288.get_parameter
22816 (&cartesian_trajectory_planner_B.rtb_signal2_idx_2);
22817
22818 // MATLABSystem: '<S15>/Get Parameter1'
22819 ParamGet_cartesian_trajectory_planner_289.get_parameter
22820 (&cartesian_trajectory_planner_B.value_p);
22821
22822 // MATLABSystem: '<S15>/Get Parameter2'
22823 ParamGet_cartesian_trajectory_planner_290.get_parameter
22824 (&cartesian_trajectory_planner_B.value_d);
22825
22826 // MATLABSystem: '<Root>/Coordinate Transformation Conversion' incorporates:
22827 // MATLABSystem: '<S15>/Get Parameter'
22828 // MATLABSystem: '<S15>/Get Parameter1'
22829 // MATLABSystem: '<S15>/Get Parameter2'
22830 // MATLABSystem: '<S15>/Get Parameter3'
22831 // MATLABSystem: '<S15>/Get Parameter4'
22832 // MATLABSystem: '<S15>/Get Parameter5'
22833 // MATLABSystem: '<S15>/Get Parameter6'
22834
22835 cartesian_trajectory_planner_B.s_vel = 1.0 / sqrt
22836 (((cartesian_trajectory_planner_B.tc * cartesian_trajectory_planner_B.tc +
22837 cartesian_trajectory_planner_B.SpMax *
22838 cartesian_trajectory_planner_B.SpMax) +
22839 cartesian_trajectory_planner_B.maxval *
22840 cartesian_trajectory_planner_B.maxval) +
22841 cartesian_trajectory_planner_B.s_m * cartesian_trajectory_planner_B.s_m);
22842 cartesian_trajectory_planner_B.tc *= cartesian_trajectory_planner_B.s_vel;
22843 cartesian_trajectory_planner_B.SpMax *= cartesian_trajectory_planner_B.s_vel;
22844 cartesian_trajectory_planner_B.maxval *=
22845 cartesian_trajectory_planner_B.s_vel;
22846 cartesian_trajectory_planner_B.tft = cartesian_trajectory_planner_B.s_m *
22847 cartesian_trajectory_planner_B.s_vel;
22848 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.tft *
22849 cartesian_trajectory_planner_B.tft;
22850 cartesian_trajectory_planner_B.s_m = cartesian_trajectory_planner_B.maxval *
22851 cartesian_trajectory_planner_B.maxval;
22852 cartesian_trajectory_planner_B.tempR[0] = 1.0 -
22853 (cartesian_trajectory_planner_B.s_m + cartesian_trajectory_planner_B.s_vel)
22854 * 2.0;
22855 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.SpMax
22856 * cartesian_trajectory_planner_B.maxval;
22857 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.tc *
22858 cartesian_trajectory_planner_B.tft;
22859 cartesian_trajectory_planner_B.tempR[1] =
22860 (cartesian_trajectory_planner_B.tcstar -
22861 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22862 cartesian_trajectory_planner_B.teta_vel =
22863 cartesian_trajectory_planner_B.SpMax * cartesian_trajectory_planner_B.tft;
22864 cartesian_trajectory_planner_B.tf = cartesian_trajectory_planner_B.tc *
22865 cartesian_trajectory_planner_B.maxval;
22866 cartesian_trajectory_planner_B.tempR[2] =
22867 (cartesian_trajectory_planner_B.teta_vel +
22868 cartesian_trajectory_planner_B.tf) * 2.0;
22869 cartesian_trajectory_planner_B.tempR[3] =
22870 (cartesian_trajectory_planner_B.tcstar +
22871 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22872 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.SpMax
22873 * cartesian_trajectory_planner_B.SpMax;
22874 cartesian_trajectory_planner_B.tempR[4] = 1.0 -
22875 (cartesian_trajectory_planner_B.tcstar +
22876 cartesian_trajectory_planner_B.s_vel) * 2.0;
22877 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.maxval
22878 * cartesian_trajectory_planner_B.tft;
22879 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.tc *
22880 cartesian_trajectory_planner_B.SpMax;
22881 cartesian_trajectory_planner_B.tempR[5] =
22882 (cartesian_trajectory_planner_B.s_vel -
22883 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22884 cartesian_trajectory_planner_B.tempR[6] =
22885 (cartesian_trajectory_planner_B.teta_vel -
22886 cartesian_trajectory_planner_B.tf) * 2.0;
22887 cartesian_trajectory_planner_B.tempR[7] =
22888 (cartesian_trajectory_planner_B.s_vel +
22889 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22890 cartesian_trajectory_planner_B.tempR[8] = 1.0 -
22891 (cartesian_trajectory_planner_B.tcstar +
22892 cartesian_trajectory_planner_B.s_m) * 2.0;
22893 for (cartesian_trajectory_planner_B.r1 = 0;
22894 cartesian_trajectory_planner_B.r1 < 3;
22895 cartesian_trajectory_planner_B.r1++) {
22896 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
22897 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 - 1] =
22898 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2
22899 - 1) * 3];
22900 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
22901 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 + 2] =
22902 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2
22903 - 1) * 3 + 1];
22904 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
22905 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 + 5] =
22906 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2
22907 - 1) * 3 + 2];
22908 }
22909
22910 memset(&cartesian_trajectory_planner_B.out[0], 0, sizeof(real_T) << 4U);
22911 for (cartesian_trajectory_planner_B.r1 = 0;
22912 cartesian_trajectory_planner_B.r1 < 3;
22913 cartesian_trajectory_planner_B.r1++) {
22914 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 << 2;
22915 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2] =
22916 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1];
22917 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2 + 1] =
22918 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1 +
22919 1];
22920 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2 + 2] =
22921 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1 +
22922 2];
22923 }
22924
22925 cartesian_trajectory_planner_B.out[15] = 1.0;
22926 cartesian_trajectory_planner_B.out[12] =
22927 cartesian_trajectory_planner_B.rtb_signal2_idx_2;
22928 cartesian_trajectory_planner_B.out[13] =
22929 cartesian_trajectory_planner_B.value_p;
22930 cartesian_trajectory_planner_B.out[14] =
22931 cartesian_trajectory_planner_B.value_d;
22932
22933 // MATLABSystem: '<S16>/Get Parameter3'
22934 ParamGet_cartesian_trajectory_planner_303.get_parameter
22935 (&cartesian_trajectory_planner_B.tc);
22936
22937 // MATLABSystem: '<S16>/Get Parameter4'
22938 ParamGet_cartesian_trajectory_planner_304.get_parameter
22939 (&cartesian_trajectory_planner_B.SpMax);
22940
22941 // MATLABSystem: '<S16>/Get Parameter5'
22942 ParamGet_cartesian_trajectory_planner_305.get_parameter
22943 (&cartesian_trajectory_planner_B.maxval);
22944
22945 // MATLABSystem: '<S16>/Get Parameter6'
22946 ParamGet_cartesian_trajectory_planner_306.get_parameter
22947 (&cartesian_trajectory_planner_B.s_m);
22948
22949 // MATLABSystem: '<S16>/Get Parameter'
22950 ParamGet_cartesian_trajectory_planner_300.get_parameter
22951 (&cartesian_trajectory_planner_B.value_g);
22952
22953 // MATLABSystem: '<S16>/Get Parameter1'
22954 ParamGet_cartesian_trajectory_planner_301.get_parameter
22955 (&cartesian_trajectory_planner_B.value_c);
22956
22957 // MATLABSystem: '<S16>/Get Parameter2'
22958 ParamGet_cartesian_trajectory_planner_302.get_parameter
22959 (&cartesian_trajectory_planner_B.value_cx);
22960
22961 // MATLABSystem: '<Root>/Coordinate Transformation Conversion1' incorporates:
22962 // MATLABSystem: '<S16>/Get Parameter'
22963 // MATLABSystem: '<S16>/Get Parameter1'
22964 // MATLABSystem: '<S16>/Get Parameter2'
22965 // MATLABSystem: '<S16>/Get Parameter3'
22966 // MATLABSystem: '<S16>/Get Parameter4'
22967 // MATLABSystem: '<S16>/Get Parameter5'
22968 // MATLABSystem: '<S16>/Get Parameter6'
22969
22970 cartesian_trajectory_planner_B.s_vel = 1.0 / sqrt
22971 (((cartesian_trajectory_planner_B.tc * cartesian_trajectory_planner_B.tc +
22972 cartesian_trajectory_planner_B.SpMax *
22973 cartesian_trajectory_planner_B.SpMax) +
22974 cartesian_trajectory_planner_B.maxval *
22975 cartesian_trajectory_planner_B.maxval) +
22976 cartesian_trajectory_planner_B.s_m * cartesian_trajectory_planner_B.s_m);
22977 cartesian_trajectory_planner_B.tc *= cartesian_trajectory_planner_B.s_vel;
22978 cartesian_trajectory_planner_B.SpMax *= cartesian_trajectory_planner_B.s_vel;
22979 cartesian_trajectory_planner_B.maxval *=
22980 cartesian_trajectory_planner_B.s_vel;
22981 cartesian_trajectory_planner_B.tft = cartesian_trajectory_planner_B.s_m *
22982 cartesian_trajectory_planner_B.s_vel;
22983 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.tft *
22984 cartesian_trajectory_planner_B.tft;
22985 cartesian_trajectory_planner_B.s_m = cartesian_trajectory_planner_B.maxval *
22986 cartesian_trajectory_planner_B.maxval;
22987 cartesian_trajectory_planner_B.tempR[0] = 1.0 -
22988 (cartesian_trajectory_planner_B.s_m + cartesian_trajectory_planner_B.s_vel)
22989 * 2.0;
22990 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.SpMax
22991 * cartesian_trajectory_planner_B.maxval;
22992 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.tc *
22993 cartesian_trajectory_planner_B.tft;
22994 cartesian_trajectory_planner_B.tempR[1] =
22995 (cartesian_trajectory_planner_B.tcstar -
22996 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
22997 cartesian_trajectory_planner_B.teta_vel =
22998 cartesian_trajectory_planner_B.SpMax * cartesian_trajectory_planner_B.tft;
22999 cartesian_trajectory_planner_B.tf = cartesian_trajectory_planner_B.tc *
23000 cartesian_trajectory_planner_B.maxval;
23001 cartesian_trajectory_planner_B.tempR[2] =
23002 (cartesian_trajectory_planner_B.teta_vel +
23003 cartesian_trajectory_planner_B.tf) * 2.0;
23004 cartesian_trajectory_planner_B.tempR[3] =
23005 (cartesian_trajectory_planner_B.tcstar +
23006 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
23007 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.SpMax
23008 * cartesian_trajectory_planner_B.SpMax;
23009 cartesian_trajectory_planner_B.tempR[4] = 1.0 -
23010 (cartesian_trajectory_planner_B.tcstar +
23011 cartesian_trajectory_planner_B.s_vel) * 2.0;
23012 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.maxval
23013 * cartesian_trajectory_planner_B.tft;
23014 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.tc *
23015 cartesian_trajectory_planner_B.SpMax;
23016 cartesian_trajectory_planner_B.tempR[5] =
23017 (cartesian_trajectory_planner_B.s_vel -
23018 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
23019 cartesian_trajectory_planner_B.tempR[6] =
23020 (cartesian_trajectory_planner_B.teta_vel -
23021 cartesian_trajectory_planner_B.tf) * 2.0;
23022 cartesian_trajectory_planner_B.tempR[7] =
23023 (cartesian_trajectory_planner_B.s_vel +
23024 cartesian_trajectory_planner_B.tetaFIN) * 2.0;
23025 cartesian_trajectory_planner_B.tempR[8] = 1.0 -
23026 (cartesian_trajectory_planner_B.tcstar +
23027 cartesian_trajectory_planner_B.s_m) * 2.0;
23028 for (cartesian_trajectory_planner_B.r1 = 0;
23029 cartesian_trajectory_planner_B.r1 < 3;
23030 cartesian_trajectory_planner_B.r1++) {
23031 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
23032 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 - 1] =
23033 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2
23034 - 1) * 3];
23035 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
23036 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 + 2] =
23037 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2
23038 - 1) * 3 + 1];
23039 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
23040 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.r2 + 5] =
23041 cartesian_trajectory_planner_B.tempR[(cartesian_trajectory_planner_B.r2
23042 - 1) * 3 + 2];
23043 }
23044
23045 memset(&cartesian_trajectory_planner_B.out_l[0], 0, sizeof(real_T) << 4U);
23046 for (cartesian_trajectory_planner_B.r1 = 0;
23047 cartesian_trajectory_planner_B.r1 < 3;
23048 cartesian_trajectory_planner_B.r1++) {
23049 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 << 2;
23050 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.r2] =
23051 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1];
23052 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.r2 + 1]
23053 = cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1
23054 + 1];
23055 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.r2 + 2]
23056 = cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.r1
23057 + 2];
23058 }
23059
23060 cartesian_trajectory_planner_B.out_l[15] = 1.0;
23061 cartesian_trajectory_planner_B.out_l[12] =
23062 cartesian_trajectory_planner_B.value_g;
23063 cartesian_trajectory_planner_B.out_l[13] =
23064 cartesian_trajectory_planner_B.value_c;
23065 cartesian_trajectory_planner_B.out_l[14] =
23066 cartesian_trajectory_planner_B.value_cx;
23067
23068 // MATLAB Function: '<Root>/Traslazione ' incorporates:
23069 // MATLABSystem: '<Root>/Get Parameter'
23070 // MATLABSystem: '<Root>/Get Parameter1'
23071
23072 cartesian_trajectory_planner_B.tc =
23073 cartesian_trajectory_planner_B.rtb_signal2_idx_0 /
23074 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23075 cartesian_trajectory_planner_B.SpMax = cartesian_trajectory_planner_B.tc *
23076 cartesian_trajectory_planner_B.rtb_signal2_idx_0;
23077
23078 // MATLAB Function: '<Root>/Completa' incorporates:
23079 // MATLAB Function: '<Root>/Traslazione '
23080
23081 memset(&cartesian_trajectory_planner_B.POSA[0], 0, sizeof(real_T) << 4U);
23082
23083 // MATLAB Function: '<Root>/Traslazione ' incorporates:
23084 // MATLAB Function: '<Root>/Completa'
23085 // MATLABSystem: '<Root>/Coordinate Transformation Conversion'
23086 // MATLABSystem: '<Root>/Coordinate Transformation Conversion1'
23087 // MATLABSystem: '<Root>/Get Parameter'
23088 // MATLABSystem: '<Root>/Get Parameter1'
23089 // MATLABSystem: '<S15>/Get Parameter'
23090 // MATLABSystem: '<S15>/Get Parameter1'
23091 // MATLABSystem: '<S15>/Get Parameter2'
23092 // MATLABSystem: '<S16>/Get Parameter'
23093 // MATLABSystem: '<S16>/Get Parameter1'
23094 // MATLABSystem: '<S16>/Get Parameter2'
23095
23096 cartesian_trajectory_planner_B.s_m = 0.0;
23097 cartesian_trajectory_planner_B.s_vel = 0.0;
23098 cartesian_trajectory_planner_B.tetaFIN =
23099 cartesian_trajectory_planner_B.value_g -
23100 cartesian_trajectory_planner_B.rtb_signal2_idx_2;
23101 cartesian_trajectory_planner_B.out_p[0] =
23102 cartesian_trajectory_planner_B.tetaFIN;
23103 cartesian_trajectory_planner_B.tft = cartesian_trajectory_planner_B.value_c
23104 - cartesian_trajectory_planner_B.value_p;
23105 cartesian_trajectory_planner_B.out_p[1] = cartesian_trajectory_planner_B.tft;
23106 cartesian_trajectory_planner_B.teta_vel =
23107 cartesian_trajectory_planner_B.value_cx -
23108 cartesian_trajectory_planner_B.value_d;
23109 cartesian_trajectory_planner_B.out_p[2] =
23110 cartesian_trajectory_planner_B.teta_vel;
23111 cartesian_trajectory_planner_B.maxval = cartesian_trajectory_pla_norm_l
23112 (cartesian_trajectory_planner_B.out_p);
23113 if (cartesian_trajectory_planner_B.SpMax >=
23114 cartesian_trajectory_planner_B.maxval) {
23115 cartesian_trajectory_planner_B.tcstar = sqrt
23116 (cartesian_trajectory_planner_B.maxval /
23117 cartesian_trajectory_planner_B.rtb_signal2_idx_1);
23118 cartesian_trajectory_planner_B.tf = 2.0 *
23119 cartesian_trajectory_planner_B.tcstar;
23120 if ((cartesian_trajectory_planner_B.time >= 0.0) &&
23121 (cartesian_trajectory_planner_B.time <
23122 cartesian_trajectory_planner_B.tcstar)) {
23123 cartesian_trajectory_planner_B.s_m = 0.5 *
23124 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23125 cartesian_trajectory_planner_B.time *
23126 cartesian_trajectory_planner_B.time;
23127 cartesian_trajectory_planner_B.s_vel =
23128 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23129 cartesian_trajectory_planner_B.time;
23130 }
23131
23132 if ((cartesian_trajectory_planner_B.time >=
23133 cartesian_trajectory_planner_B.tcstar) &&
23134 (cartesian_trajectory_planner_B.time <=
23135 cartesian_trajectory_planner_B.tf)) {
23136 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.tf
23137 - cartesian_trajectory_planner_B.time;
23138 cartesian_trajectory_planner_B.s_m =
23139 cartesian_trajectory_planner_B.maxval - 0.5 *
23140 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23141 cartesian_trajectory_planner_B.s_vel *
23142 cartesian_trajectory_planner_B.s_vel;
23143 cartesian_trajectory_planner_B.s_vel *=
23144 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23145 }
23146
23147 if (cartesian_trajectory_planner_B.time >
23148 cartesian_trajectory_planner_B.tf) {
23149 cartesian_trajectory_planner_B.s_m =
23150 cartesian_trajectory_planner_B.maxval;
23151 cartesian_trajectory_planner_B.s_vel = 0.0;
23152 }
23153 }
23154
23155 if (cartesian_trajectory_planner_B.SpMax <
23156 cartesian_trajectory_planner_B.maxval) {
23157 cartesian_trajectory_planner_B.tcstar =
23158 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23159 cartesian_trajectory_planner_B.tc;
23160 cartesian_trajectory_planner_B.tf = (cartesian_trajectory_planner_B.maxval
23161 - cartesian_trajectory_planner_B.tcstar *
23162 cartesian_trajectory_planner_B.tc) * (1.0 /
23163 cartesian_trajectory_planner_B.rtb_signal2_idx_0) + 2.0 *
23164 cartesian_trajectory_planner_B.tc;
23165 if ((cartesian_trajectory_planner_B.time >= 0.0) &&
23166 (cartesian_trajectory_planner_B.time <=
23167 cartesian_trajectory_planner_B.tc)) {
23168 cartesian_trajectory_planner_B.s_m = 0.5 *
23169 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23170 cartesian_trajectory_planner_B.time *
23171 cartesian_trajectory_planner_B.time;
23172 cartesian_trajectory_planner_B.s_vel =
23173 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23174 cartesian_trajectory_planner_B.time;
23175 }
23176
23177 cartesian_trajectory_planner_B.d = cartesian_trajectory_planner_B.tf -
23178 cartesian_trajectory_planner_B.tc;
23179 if ((cartesian_trajectory_planner_B.time >
23180 cartesian_trajectory_planner_B.tc) &&
23181 (cartesian_trajectory_planner_B.time <=
23182 cartesian_trajectory_planner_B.d)) {
23183 cartesian_trajectory_planner_B.s_m =
23184 (cartesian_trajectory_planner_B.time -
23185 cartesian_trajectory_planner_B.tc / 2.0) *
23186 cartesian_trajectory_planner_B.tcstar;
23187 cartesian_trajectory_planner_B.s_vel =
23188 cartesian_trajectory_planner_B.rtb_signal2_idx_0;
23189 }
23190
23191 if ((cartesian_trajectory_planner_B.time >
23192 cartesian_trajectory_planner_B.d) &&
23193 (cartesian_trajectory_planner_B.time <=
23194 cartesian_trajectory_planner_B.tf)) {
23195 cartesian_trajectory_planner_B.out_p[0] =
23196 cartesian_trajectory_planner_B.tetaFIN;
23197 cartesian_trajectory_planner_B.out_p[1] =
23198 cartesian_trajectory_planner_B.tft;
23199 cartesian_trajectory_planner_B.out_p[2] =
23200 cartesian_trajectory_planner_B.teta_vel;
23201 cartesian_trajectory_planner_B.s_m = cartesian_trajectory_pla_norm_l
23202 (cartesian_trajectory_planner_B.out_p) - 0.5 *
23203 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23204 (cartesian_trajectory_planner_B.tf -
23205 cartesian_trajectory_planner_B.time) *
23206 (cartesian_trajectory_planner_B.tf -
23207 cartesian_trajectory_planner_B.time);
23208 cartesian_trajectory_planner_B.s_vel =
23209 (cartesian_trajectory_planner_B.tf -
23210 cartesian_trajectory_planner_B.time) *
23211 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23212 }
23213
23214 if (cartesian_trajectory_planner_B.time >
23215 cartesian_trajectory_planner_B.tf) {
23216 cartesian_trajectory_planner_B.s_m =
23217 cartesian_trajectory_planner_B.maxval;
23218 cartesian_trajectory_planner_B.s_vel = 0.0;
23219 }
23220 }
23221
23222 cartesian_trajectory_planner_B.P_k[0] = cartesian_trajectory_planner_B.s_m /
23223 cartesian_trajectory_planner_B.maxval *
23224 cartesian_trajectory_planner_B.tetaFIN +
23225 cartesian_trajectory_planner_B.rtb_signal2_idx_2;
23226 cartesian_trajectory_planner_B.out_p[0] =
23227 cartesian_trajectory_planner_B.tetaFIN;
23228 cartesian_trajectory_planner_B.out_b[0] =
23229 cartesian_trajectory_planner_B.tetaFIN;
23230 cartesian_trajectory_planner_B.out_p[1] = cartesian_trajectory_planner_B.tft;
23231 cartesian_trajectory_planner_B.out_b[1] = cartesian_trajectory_planner_B.tft;
23232 cartesian_trajectory_planner_B.out_p[2] =
23233 cartesian_trajectory_planner_B.teta_vel;
23234 cartesian_trajectory_planner_B.out_b[2] =
23235 cartesian_trajectory_planner_B.teta_vel;
23236 cartesian_trajectory_planner_B.P_k[1] = cartesian_trajectory_planner_B.s_m /
23237 cartesian_trajectory_pla_norm_l(cartesian_trajectory_planner_B.out_p) *
23238 cartesian_trajectory_planner_B.tft +
23239 cartesian_trajectory_planner_B.value_p;
23240 cartesian_trajectory_planner_B.P_k[2] = cartesian_trajectory_planner_B.s_m /
23241 cartesian_trajectory_pla_norm_l(cartesian_trajectory_planner_B.out_b) *
23242 cartesian_trajectory_planner_B.teta_vel +
23243 cartesian_trajectory_planner_B.value_d;
23244 cartesian_trajectory_planner_B.POSA[15] = 1.0;
23245 for (cartesian_trajectory_planner_B.r1 = 0;
23246 cartesian_trajectory_planner_B.r1 < 3;
23247 cartesian_trajectory_planner_B.r1++) {
23248 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r1 + 12]
23249 = cartesian_trajectory_planner_B.P_k[cartesian_trajectory_planner_B.r1];
23250 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 << 2;
23251 cartesian_trajectory_planner_B.tempR[3 * cartesian_trajectory_planner_B.r1]
23252 = cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.r2];
23253 cartesian_trajectory_planner_B.tempR[3 * cartesian_trajectory_planner_B.r1
23254 + 1] =
23255 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.r2 +
23256 1];
23257 cartesian_trajectory_planner_B.tempR[3 * cartesian_trajectory_planner_B.r1
23258 + 2] =
23259 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.r2 +
23260 2];
23261 }
23262
23263 cartesian_trajectory_planner_B.r1 = 0;
23264 cartesian_trajectory_planner_B.r2 = 1;
23265 cartesian_trajectory_planner_B.r3 = 2;
23266 cartesian_trajectory_planner_B.maxval = fabs
23267 (cartesian_trajectory_planner_B.out_l[0]);
23268 cartesian_trajectory_planner_B.s_m = fabs
23269 (cartesian_trajectory_planner_B.out_l[1]);
23270 if (cartesian_trajectory_planner_B.s_m >
23271 cartesian_trajectory_planner_B.maxval) {
23272 cartesian_trajectory_planner_B.maxval = cartesian_trajectory_planner_B.s_m;
23273 cartesian_trajectory_planner_B.r1 = 1;
23274 cartesian_trajectory_planner_B.r2 = 0;
23275 }
23276
23277 if (fabs(cartesian_trajectory_planner_B.out_l[2]) >
23278 cartesian_trajectory_planner_B.maxval) {
23279 cartesian_trajectory_planner_B.r1 = 2;
23280 cartesian_trajectory_planner_B.r2 = 1;
23281 cartesian_trajectory_planner_B.r3 = 0;
23282 }
23283
23284 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2] =
23285 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.r2] /
23286 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.r1];
23287 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3] /=
23288 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1];
23289 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 3] -=
23290 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 3]
23291 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23292 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 3] -=
23293 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 3]
23294 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23295 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 6] -=
23296 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 6]
23297 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23298 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 6] -=
23299 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 6]
23300 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23301 if (fabs
23302 (cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3
23303 + 3]) > fabs
23304 (cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2
23305 + 3])) {
23306 cartesian_trajectory_planner_B.rtemp = cartesian_trajectory_planner_B.r2;
23307 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r3;
23308 cartesian_trajectory_planner_B.r3 = cartesian_trajectory_planner_B.rtemp;
23309 }
23310
23311 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 3] /=
23312 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 3];
23313 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 6] -=
23314 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 3]
23315 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 +
23316 6];
23317 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r1]
23318 = cartesian_trajectory_planner_B.out[0] /
23319 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1];
23320 cartesian_trajectory_planner_B.maxval =
23321 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 3];
23322 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r2]
23323 = cartesian_trajectory_planner_B.out[4] -
23324 cartesian_trajectory_planner_B.Matrot[3 *
23325 cartesian_trajectory_planner_B.r1] * cartesian_trajectory_planner_B.maxval;
23326 cartesian_trajectory_planner_B.s_m =
23327 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1 + 6];
23328 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r3]
23329 = cartesian_trajectory_planner_B.out[8] -
23330 cartesian_trajectory_planner_B.Matrot[3 *
23331 cartesian_trajectory_planner_B.r1] * cartesian_trajectory_planner_B.s_m;
23332 cartesian_trajectory_planner_B.tcstar =
23333 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 3];
23334 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r2]
23335 /= cartesian_trajectory_planner_B.tcstar;
23336 cartesian_trajectory_planner_B.tetaFIN =
23337 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2 + 6];
23338 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r3]
23339 -= cartesian_trajectory_planner_B.Matrot[3 *
23340 cartesian_trajectory_planner_B.r2] *
23341 cartesian_trajectory_planner_B.tetaFIN;
23342 cartesian_trajectory_planner_B.tft =
23343 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 6];
23344 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r3]
23345 /= cartesian_trajectory_planner_B.tft;
23346 cartesian_trajectory_planner_B.teta_vel =
23347 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3 + 3];
23348 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r2]
23349 -= cartesian_trajectory_planner_B.Matrot[3 *
23350 cartesian_trajectory_planner_B.r3] *
23351 cartesian_trajectory_planner_B.teta_vel;
23352 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r1]
23353 -= cartesian_trajectory_planner_B.Matrot[3 *
23354 cartesian_trajectory_planner_B.r3] *
23355 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23356 cartesian_trajectory_planner_B.Matrot[3 * cartesian_trajectory_planner_B.r1]
23357 -= cartesian_trajectory_planner_B.Matrot[3 *
23358 cartesian_trajectory_planner_B.r2] *
23359 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23360 cartesian_trajectory_planner_B.rtemp = 3 * cartesian_trajectory_planner_B.r1
23361 + 1;
23362 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] =
23363 cartesian_trajectory_planner_B.out[1] /
23364 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1];
23365 cartesian_trajectory_planner_B.Matrot_tmp = 3 *
23366 cartesian_trajectory_planner_B.r2 + 1;
23367 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23368 = cartesian_trajectory_planner_B.out[5] -
23369 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp]
23370 * cartesian_trajectory_planner_B.maxval;
23371 cartesian_trajectory_planner_B.Matrot_tmp_h = 3 *
23372 cartesian_trajectory_planner_B.r3 + 1;
23373 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23374 = cartesian_trajectory_planner_B.out[9] -
23375 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp]
23376 * cartesian_trajectory_planner_B.s_m;
23377 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23378 /= cartesian_trajectory_planner_B.tcstar;
23379 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23380 -=
23381 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23382 * cartesian_trajectory_planner_B.tetaFIN;
23383 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23384 /= cartesian_trajectory_planner_B.tft;
23385 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23386 -=
23387 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23388 * cartesian_trajectory_planner_B.teta_vel;
23389 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] -=
23390 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23391 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23392 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] -=
23393 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23394 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23395 cartesian_trajectory_planner_B.rtemp = 3 * cartesian_trajectory_planner_B.r1
23396 + 2;
23397 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] =
23398 cartesian_trajectory_planner_B.out[2] /
23399 cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r1];
23400 cartesian_trajectory_planner_B.Matrot_tmp = 3 *
23401 cartesian_trajectory_planner_B.r2 + 2;
23402 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23403 = cartesian_trajectory_planner_B.out[6] -
23404 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp]
23405 * cartesian_trajectory_planner_B.maxval;
23406 cartesian_trajectory_planner_B.Matrot_tmp_h = 3 *
23407 cartesian_trajectory_planner_B.r3 + 2;
23408 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23409 = cartesian_trajectory_planner_B.out[10] -
23410 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp]
23411 * cartesian_trajectory_planner_B.s_m;
23412 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23413 /= cartesian_trajectory_planner_B.tcstar;
23414 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23415 -=
23416 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23417 * cartesian_trajectory_planner_B.tetaFIN;
23418 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23419 /= cartesian_trajectory_planner_B.tft;
23420 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23421 -=
23422 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23423 * cartesian_trajectory_planner_B.teta_vel;
23424 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] -=
23425 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp_h]
23426 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r3];
23427 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.rtemp] -=
23428 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.Matrot_tmp]
23429 * cartesian_trajectory_planner_B.tempR[cartesian_trajectory_planner_B.r2];
23430 cartesian_trajectory_planner_B.tetaFIN = acos
23431 ((((cartesian_trajectory_planner_B.Matrot[0] +
23432 cartesian_trajectory_planner_B.Matrot[4]) +
23433 cartesian_trajectory_planner_B.Matrot[8]) - 1.0) / 2.0);
23434 cartesian_trajectory_planner_B.maxval = 1.0 / (2.0 * sin
23435 (cartesian_trajectory_planner_B.tetaFIN));
23436 cartesian_trajectory_planner_B.P_k[0] =
23437 (cartesian_trajectory_planner_B.Matrot[5] -
23438 cartesian_trajectory_planner_B.Matrot[7]) *
23439 cartesian_trajectory_planner_B.maxval;
23440 cartesian_trajectory_planner_B.P_k[1] =
23441 (cartesian_trajectory_planner_B.Matrot[6] -
23442 cartesian_trajectory_planner_B.Matrot[2]) *
23443 cartesian_trajectory_planner_B.maxval;
23444 cartesian_trajectory_planner_B.P_k[2] =
23445 (cartesian_trajectory_planner_B.Matrot[1] -
23446 cartesian_trajectory_planner_B.Matrot[3]) *
23447 cartesian_trajectory_planner_B.maxval;
23448 cartesian_trajectory_planner_B.s_m = 0.0;
23449 cartesian_trajectory_planner_B.teta_vel = 0.0;
23450 if (cartesian_trajectory_planner_B.SpMax >=
23451 cartesian_trajectory_planner_B.tetaFIN) {
23452 cartesian_trajectory_planner_B.tcstar = sqrt
23453 (cartesian_trajectory_planner_B.tetaFIN /
23454 cartesian_trajectory_planner_B.rtb_signal2_idx_1);
23455 cartesian_trajectory_planner_B.tft = 2.0 *
23456 cartesian_trajectory_planner_B.tcstar;
23457 if ((cartesian_trajectory_planner_B.time >= 0.0) &&
23458 (cartesian_trajectory_planner_B.time <
23459 cartesian_trajectory_planner_B.tcstar)) {
23460 cartesian_trajectory_planner_B.s_m = 0.5 *
23461 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23462 cartesian_trajectory_planner_B.time *
23463 cartesian_trajectory_planner_B.time;
23464 cartesian_trajectory_planner_B.teta_vel =
23465 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23466 cartesian_trajectory_planner_B.time;
23467 }
23468
23469 if ((cartesian_trajectory_planner_B.time >=
23470 cartesian_trajectory_planner_B.tcstar) &&
23471 (cartesian_trajectory_planner_B.time <=
23472 cartesian_trajectory_planner_B.tft)) {
23473 cartesian_trajectory_planner_B.maxval =
23474 cartesian_trajectory_planner_B.tft -
23475 cartesian_trajectory_planner_B.time;
23476 cartesian_trajectory_planner_B.s_m =
23477 cartesian_trajectory_planner_B.tetaFIN - 0.5 *
23478 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23479 cartesian_trajectory_planner_B.maxval *
23480 cartesian_trajectory_planner_B.maxval;
23481 cartesian_trajectory_planner_B.teta_vel =
23482 cartesian_trajectory_planner_B.maxval *
23483 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23484 }
23485
23486 if (cartesian_trajectory_planner_B.time >
23487 cartesian_trajectory_planner_B.tft) {
23488 cartesian_trajectory_planner_B.s_m =
23489 cartesian_trajectory_planner_B.tetaFIN;
23490 cartesian_trajectory_planner_B.teta_vel = 0.0;
23491 }
23492 }
23493
23494 if (cartesian_trajectory_planner_B.SpMax <
23495 cartesian_trajectory_planner_B.tetaFIN) {
23496 cartesian_trajectory_planner_B.tft =
23497 (cartesian_trajectory_planner_B.tetaFIN -
23498 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23499 cartesian_trajectory_planner_B.tc * cartesian_trajectory_planner_B.tc) *
23500 (1.0 / cartesian_trajectory_planner_B.rtb_signal2_idx_0) + 2.0 *
23501 cartesian_trajectory_planner_B.tc;
23502 if ((cartesian_trajectory_planner_B.time >= 0.0) &&
23503 (cartesian_trajectory_planner_B.time <=
23504 cartesian_trajectory_planner_B.tc)) {
23505 cartesian_trajectory_planner_B.s_m = 0.5 *
23506 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23507 cartesian_trajectory_planner_B.time *
23508 cartesian_trajectory_planner_B.time;
23509 cartesian_trajectory_planner_B.teta_vel =
23510 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23511 cartesian_trajectory_planner_B.time;
23512 }
23513
23514 cartesian_trajectory_planner_B.maxval = cartesian_trajectory_planner_B.tft
23515 - cartesian_trajectory_planner_B.tc;
23516 if ((cartesian_trajectory_planner_B.time >
23517 cartesian_trajectory_planner_B.tc) &&
23518 (cartesian_trajectory_planner_B.time <=
23519 cartesian_trajectory_planner_B.maxval)) {
23520 cartesian_trajectory_planner_B.s_m =
23521 (cartesian_trajectory_planner_B.time -
23522 cartesian_trajectory_planner_B.tc / 2.0) *
23523 (cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23524 cartesian_trajectory_planner_B.tc);
23525 cartesian_trajectory_planner_B.teta_vel =
23526 cartesian_trajectory_planner_B.rtb_signal2_idx_0;
23527 }
23528
23529 if ((cartesian_trajectory_planner_B.time >
23530 cartesian_trajectory_planner_B.maxval) &&
23531 (cartesian_trajectory_planner_B.time <=
23532 cartesian_trajectory_planner_B.tft)) {
23533 cartesian_trajectory_planner_B.s_m =
23534 cartesian_trajectory_planner_B.tetaFIN - 0.5 *
23535 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23536 (cartesian_trajectory_planner_B.tft -
23537 cartesian_trajectory_planner_B.time) *
23538 (cartesian_trajectory_planner_B.tft -
23539 cartesian_trajectory_planner_B.time);
23540 cartesian_trajectory_planner_B.teta_vel =
23541 (cartesian_trajectory_planner_B.tft -
23542 cartesian_trajectory_planner_B.time) *
23543 cartesian_trajectory_planner_B.rtb_signal2_idx_0;
23544 }
23545
23546 if (cartesian_trajectory_planner_B.time >
23547 cartesian_trajectory_planner_B.tft) {
23548 cartesian_trajectory_planner_B.s_m =
23549 cartesian_trajectory_planner_B.tetaFIN;
23550 cartesian_trajectory_planner_B.teta_vel = 0.0;
23551 }
23552 }
23553
23554 cartesian_trajectory_planner_B.Vel[0] =
23555 (cartesian_trajectory_planner_B.rtb_signal2_idx_2 -
23556 cartesian_trajectory_planner_B.value_g) *
23557 cartesian_trajectory_planner_B.s_vel;
23558 cartesian_trajectory_planner_B.Vel[3] =
23559 cartesian_trajectory_planner_B.teta_vel *
23560 cartesian_trajectory_planner_B.P_k[0];
23561 cartesian_trajectory_planner_B.Vel[1] =
23562 (cartesian_trajectory_planner_B.value_p -
23563 cartesian_trajectory_planner_B.value_c) *
23564 cartesian_trajectory_planner_B.s_vel;
23565 cartesian_trajectory_planner_B.Vel[4] =
23566 cartesian_trajectory_planner_B.teta_vel *
23567 cartesian_trajectory_planner_B.P_k[1];
23568 cartesian_trajectory_planner_B.Vel[2] =
23569 (cartesian_trajectory_planner_B.value_d -
23570 cartesian_trajectory_planner_B.value_cx) *
23571 cartesian_trajectory_planner_B.s_vel;
23572 cartesian_trajectory_planner_B.Vel[5] =
23573 cartesian_trajectory_planner_B.teta_vel *
23574 cartesian_trajectory_planner_B.P_k[2];
23575 cartesian_trajectory_planner_B.rtb_signal2_idx_0 =
23576 cartesian_trajectory_planner_B.P_k[0];
23577 cartesian_trajectory_planner_B.rtb_signal2_idx_1 =
23578 cartesian_trajectory_planner_B.P_k[1];
23579 cartesian_trajectory_planner_B.rtb_signal2_idx_2 =
23580 cartesian_trajectory_planner_B.P_k[2];
23581
23582 // MATLABSystem: '<Root>/Coordinate Transformation Conversion2' incorporates:
23583 // MATLAB Function: '<Root>/Traslazione '
23584
23585 cartesian_trajectory_planner_B.s_vel = 1.0 / sqrt
23586 ((cartesian_trajectory_planner_B.rtb_signal2_idx_0 *
23587 cartesian_trajectory_planner_B.rtb_signal2_idx_0 +
23588 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23589 cartesian_trajectory_planner_B.rtb_signal2_idx_1) +
23590 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23591 cartesian_trajectory_planner_B.rtb_signal2_idx_2);
23592 cartesian_trajectory_planner_B.P_k[0] =
23593 cartesian_trajectory_planner_B.rtb_signal2_idx_0 *
23594 cartesian_trajectory_planner_B.s_vel;
23595 cartesian_trajectory_planner_B.P_k[1] =
23596 cartesian_trajectory_planner_B.rtb_signal2_idx_1 *
23597 cartesian_trajectory_planner_B.s_vel;
23598 cartesian_trajectory_planner_B.P_k[2] =
23599 cartesian_trajectory_planner_B.rtb_signal2_idx_2 *
23600 cartesian_trajectory_planner_B.s_vel;
23601 cartesian_trajectory_planner_B.rtb_signal2_idx_0 = cos
23602 (cartesian_trajectory_planner_B.s_m);
23603 cartesian_trajectory_planner_B.rtb_signal2_idx_1 = sin
23604 (cartesian_trajectory_planner_B.s_m);
23605 cartesian_trajectory_planner_B.tempR[0] =
23606 cartesian_trajectory_planner_B.P_k[0] *
23607 cartesian_trajectory_planner_B.P_k[0] * (1.0 -
23608 cartesian_trajectory_planner_B.rtb_signal2_idx_0) +
23609 cartesian_trajectory_planner_B.rtb_signal2_idx_0;
23610 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.P_k[1]
23611 * cartesian_trajectory_planner_B.P_k[0] * (1.0 -
23612 cartesian_trajectory_planner_B.rtb_signal2_idx_0);
23613 cartesian_trajectory_planner_B.s_m = cartesian_trajectory_planner_B.P_k[2] *
23614 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23615 cartesian_trajectory_planner_B.tempR[1] =
23616 cartesian_trajectory_planner_B.s_vel - cartesian_trajectory_planner_B.s_m;
23617 cartesian_trajectory_planner_B.tcstar = cartesian_trajectory_planner_B.P_k[2]
23618 * cartesian_trajectory_planner_B.P_k[0] * (1.0 -
23619 cartesian_trajectory_planner_B.rtb_signal2_idx_0);
23620 cartesian_trajectory_planner_B.tetaFIN = cartesian_trajectory_planner_B.P_k
23621 [1] * cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23622 cartesian_trajectory_planner_B.tempR[2] =
23623 cartesian_trajectory_planner_B.tcstar +
23624 cartesian_trajectory_planner_B.tetaFIN;
23625 cartesian_trajectory_planner_B.tempR[3] =
23626 cartesian_trajectory_planner_B.s_vel + cartesian_trajectory_planner_B.s_m;
23627 cartesian_trajectory_planner_B.tempR[4] =
23628 cartesian_trajectory_planner_B.P_k[1] *
23629 cartesian_trajectory_planner_B.P_k[1] * (1.0 -
23630 cartesian_trajectory_planner_B.rtb_signal2_idx_0) +
23631 cartesian_trajectory_planner_B.rtb_signal2_idx_0;
23632 cartesian_trajectory_planner_B.s_vel = cartesian_trajectory_planner_B.P_k[2]
23633 * cartesian_trajectory_planner_B.P_k[1] * (1.0 -
23634 cartesian_trajectory_planner_B.rtb_signal2_idx_0);
23635 cartesian_trajectory_planner_B.s_m = cartesian_trajectory_planner_B.P_k[0] *
23636 cartesian_trajectory_planner_B.rtb_signal2_idx_1;
23637 cartesian_trajectory_planner_B.tempR[5] =
23638 cartesian_trajectory_planner_B.s_vel - cartesian_trajectory_planner_B.s_m;
23639 cartesian_trajectory_planner_B.tempR[6] =
23640 cartesian_trajectory_planner_B.tcstar -
23641 cartesian_trajectory_planner_B.tetaFIN;
23642 cartesian_trajectory_planner_B.tempR[7] =
23643 cartesian_trajectory_planner_B.s_vel + cartesian_trajectory_planner_B.s_m;
23644 cartesian_trajectory_planner_B.tempR[8] =
23645 cartesian_trajectory_planner_B.P_k[2] *
23646 cartesian_trajectory_planner_B.P_k[2] * (1.0 -
23647 cartesian_trajectory_planner_B.rtb_signal2_idx_0) +
23648 cartesian_trajectory_planner_B.rtb_signal2_idx_0;
23649 for (cartesian_trajectory_planner_B.r1 = 0;
23650 cartesian_trajectory_planner_B.r1 < 3;
23651 cartesian_trajectory_planner_B.r1++) {
23652 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
23653 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.r2 -
23654 1] = cartesian_trajectory_planner_B.tempR
23655 [(cartesian_trajectory_planner_B.r2 - 1) * 3];
23656 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
23657 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.r2 +
23658 2] = cartesian_trajectory_planner_B.tempR
23659 [(cartesian_trajectory_planner_B.r2 - 1) * 3 + 1];
23660 cartesian_trajectory_planner_B.r2 = cartesian_trajectory_planner_B.r1 + 1;
23661 cartesian_trajectory_planner_B.Matrot[cartesian_trajectory_planner_B.r2 +
23662 5] = cartesian_trajectory_planner_B.tempR
23663 [(cartesian_trajectory_planner_B.r2 - 1) * 3 + 2];
23664 }
23665
23666 // MATLAB Function: '<Root>/Completa' incorporates:
23667 // MATLABSystem: '<Root>/Coordinate Transformation Conversion'
23668 // MATLABSystem: '<Root>/Coordinate Transformation Conversion2'
23669
23670 for (cartesian_trajectory_planner_B.r1 = 0;
23671 cartesian_trajectory_planner_B.r1 < 3;
23672 cartesian_trajectory_planner_B.r1++) {
23673 for (cartesian_trajectory_planner_B.r2 = 0;
23674 cartesian_trajectory_planner_B.r2 < 3;
23675 cartesian_trajectory_planner_B.r2++) {
23676 cartesian_trajectory_planner_B.r3 = cartesian_trajectory_planner_B.r2 +
23677 (cartesian_trajectory_planner_B.r1 << 2);
23678 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r3] =
23679 0.0;
23680 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r3] +=
23681 cartesian_trajectory_planner_B.Matrot[3 *
23682 cartesian_trajectory_planner_B.r1] *
23683 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2];
23684 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r3] +=
23685 cartesian_trajectory_planner_B.Matrot[3 *
23686 cartesian_trajectory_planner_B.r1 + 1] *
23687 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2 +
23688 4];
23689 cartesian_trajectory_planner_B.POSA[cartesian_trajectory_planner_B.r3] +=
23690 cartesian_trajectory_planner_B.Matrot[3 *
23691 cartesian_trajectory_planner_B.r1 + 2] *
23692 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.r2 +
23693 8];
23694 }
23695 }
23696
23697 // MATLABSystem: '<S6>/MATLAB System' incorporates:
23698 // MATLABSystem: '<S12>/Get Parameter'
23699 // MATLABSystem: '<S12>/Get Parameter1'
23700 // MATLABSystem: '<S12>/Get Parameter2'
23701 // MATLABSystem: '<S12>/Get Parameter3'
23702 // MATLABSystem: '<S12>/Get Parameter4'
23703 // MATLABSystem: '<S12>/Get Parameter5'
23704
23705 ParamGet_cartesian_trajectory_planner_312.get_parameter
23706 (&cartesian_trajectory_planner_B.value[0]);
23707 ParamGet_cartesian_trajectory_planner_313.get_parameter
23708 (&cartesian_trajectory_planner_B.value[1]);
23709 ParamGet_cartesian_trajectory_planner_314.get_parameter
23710 (&cartesian_trajectory_planner_B.value[2]);
23711 ParamGet_cartesian_trajectory_planner_315.get_parameter
23712 (&cartesian_trajectory_planner_B.value[3]);
23713 ParamGet_cartesian_trajectory_planner_316.get_parameter
23714 (&cartesian_trajectory_planner_B.value[4]);
23715 ParamGet_cartesian_trajectory_planner_317.get_parameter
23716 (&cartesian_trajectory_planner_B.value[5]);
23717 if (rtmIsMajorTimeStep(cartesian_trajectory_planner_M)) {
23718 // Delay: '<Root>/Delay'
23719 for (cartesian_trajectory_planner_B.r2 = 0;
23720 cartesian_trajectory_planner_B.r2 < 6;
23721 cartesian_trajectory_planner_B.r2++) {
23722 cartesian_trajectory_planner_B.Delay[cartesian_trajectory_planner_B.r2] =
23723 cartesian_trajectory_planner_DW.Delay_DSTATE[cartesian_trajectory_planner_B.r2];
23724 }
23725
23726 // End of Delay: '<Root>/Delay'
23727 }
23728
23729 // MATLABSystem: '<S6>/MATLAB System'
23730 obj = &cartesian_trajectory_planner_DW.obj;
23731 if (cartesian_trajectory_planner_DW.obj.IKInternal.isInitialized != 1) {
23732 cartesian_trajectory_planner_DW.obj.IKInternal.isSetupComplete = false;
23733 cartesian_trajectory_planner_DW.obj.IKInternal.isInitialized = 1;
23734 car_inverseKinematics_setupImpl
23735 (&cartesian_trajectory_planner_DW.obj.IKInternal,
23736 &cartesian_trajectory_planner_DW.gobj_85);
23737 obj->IKInternal.isSetupComplete = true;
23738 }
23739
23740 cartesian_trajec_emxInit_real_T(&b, 2);
23741
23742 // MATLABSystem: '<S6>/MATLAB System'
23743 cart_inverseKinematics_stepImpl(&obj->IKInternal,
23744 cartesian_trajectory_planner_B.POSA, cartesian_trajectory_planner_B.value,
23745 cartesian_trajectory_planner_B.Delay,
23746 cartesian_trajectory_planner_B.MATLABSystem_o1);
23747
23748 // MATLABSystem: '<S3>/MATLAB System'
23749 RigidBodyTree_geometricJacobian
23750 (&cartesian_trajectory_planner_DW.obj_g.TreeInternal,
23751 cartesian_trajectory_planner_B.MATLABSystem_o1, b);
23752 cartesian_trajecto_MATLABSystem
23753 (cartesian_trajectory_planner_B.MATLABSystem_o1,
23754 &cartesian_trajectory_planner_B.MATLABSystem_d,
23755 &cartesian_trajectory_planner_DW.MATLABSystem_d);
23756
23757 // Product: '<Root>/Reciprocal' incorporates:
23758 // MATLABSystem: '<S3>/MATLAB System'
23759
23760 rt_invd6x6_snf(b->data, cartesian_trajectory_planner_B.dv);
23761 cartesian_trajec_emxFree_real_T(&b);
23762
23763 // MATLAB Function: '<Root>/MATLAB Function' incorporates:
23764 // Constant: '<S1>/Constant'
23765
23766 cartesian_trajectory_planner_B.msg =
23767 cartesian_trajectory_planner_P.Constant_Value;
23768 cartesian_trajectory_planner_B.msg.Velocities_SL_Info.CurrentLength = 6U;
23769 cartesian_trajectory_planner_B.msg.Positions_SL_Info.CurrentLength = 6U;
23770 for (cartesian_trajectory_planner_B.r2 = 0;
23771 cartesian_trajectory_planner_B.r2 < 6;
23772 cartesian_trajectory_planner_B.r2++) {
23773 // Product: '<Root>/MatrixMultiply1' incorporates:
23774 // Product: '<Root>/Reciprocal'
23775
23776 cartesian_trajectory_planner_B.MatrixMultiply1[cartesian_trajectory_planner_B.r2]
23777 = 0.0;
23778 for (cartesian_trajectory_planner_B.r1 = 0;
23779 cartesian_trajectory_planner_B.r1 < 6;
23780 cartesian_trajectory_planner_B.r1++) {
23781 cartesian_trajectory_planner_B.MatrixMultiply1[cartesian_trajectory_planner_B.r2]
23782 += cartesian_trajectory_planner_B.dv[6 *
23783 cartesian_trajectory_planner_B.r1 + cartesian_trajectory_planner_B.r2]
23784 * cartesian_trajectory_planner_B.Vel[cartesian_trajectory_planner_B.r1];
23785 }
23786
23787 // End of Product: '<Root>/MatrixMultiply1'
23788
23789 // MATLAB Function: '<Root>/MATLAB Function'
23790 cartesian_trajectory_planner_B.msg.Velocities[cartesian_trajectory_planner_B.r2]
23791 =
23792 cartesian_trajectory_planner_B.MatrixMultiply1[cartesian_trajectory_planner_B.r2];
23793 cartesian_trajectory_planner_B.msg.Positions[cartesian_trajectory_planner_B.r2]
23794 =
23795 cartesian_trajectory_planner_B.MATLABSystem_o1[cartesian_trajectory_planner_B.r2];
23796 }
23797
23798 // Outputs for Atomic SubSystem: '<Root>/Publish'
23799 // MATLABSystem: '<S9>/SinkBlock'
23800 Pub_cartesian_trajectory_planner_278.publish
23801 (&cartesian_trajectory_planner_B.msg);
23802
23803 // End of Outputs for SubSystem: '<Root>/Publish'
23804
23805 // Integrator: '<Root>/Integrator'
23806 for (cartesian_trajectory_planner_B.r2 = 0;
23807 cartesian_trajectory_planner_B.r2 < 6;
23808 cartesian_trajectory_planner_B.r2++) {
23809 cartesian_trajectory_planner_B.Integrator[cartesian_trajectory_planner_B.r2]
23810 =
23811 cartesian_trajectory_planner_X.Integrator_CSTATE[cartesian_trajectory_planner_B.r2];
23812 }
23813
23814 // End of Integrator: '<Root>/Integrator'
23815 cartesian_trajecto_MATLABSystem(cartesian_trajectory_planner_B.Integrator,
23816 &cartesian_trajectory_planner_B.MATLABSystem_h,
23817 &cartesian_trajectory_planner_DW.MATLABSystem_h);
23818 if (rtmIsMajorTimeStep(cartesian_trajectory_planner_M)) {
23819 // MATLABSystem: '<S11>/Get Parameter'
23820 ParamGet_cartesian_trajectory_planner_283.get_parameter
23821 (&cartesian_trajectory_planner_B.rtb_signal2_idx_0);
23822
23823 // MATLABSystem: '<S11>/Get Parameter1'
23824 ParamGet_cartesian_trajectory_planner_284.get_parameter
23825 (&cartesian_trajectory_planner_B.rtb_signal2_idx_0);
23826 }
23827 }
23828
23829 if (rtmIsMajorTimeStep(cartesian_trajectory_planner_M)) {
23830 int32_T i;
23831 if (rtmIsMajorTimeStep(cartesian_trajectory_planner_M)) {
23832 // Update for Delay: '<Root>/Delay'
23833 for (i = 0; i < 6; i++) {
23834 cartesian_trajectory_planner_DW.Delay_DSTATE[i] =
23835 cartesian_trajectory_planner_B.MATLABSystem_o1[i];
23836 }
23837
23838 // End of Update for Delay: '<Root>/Delay'
23839 }
23840 } // end MajorTimeStep
23841
23842 if (rtmIsMajorTimeStep(cartesian_trajectory_planner_M)) {
23843 rt_ertODEUpdateContinuousStates(&cartesian_trajectory_planner_M->solverInfo);
23844
23845 // Update absolute time for base rate
23846 // The "clockTick0" counts the number of times the code of this task has
23847 // been executed. The absolute time is the multiplication of "clockTick0"
23848 // and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
23849 // overflow during the application lifespan selected.
23850
23851 ++cartesian_trajectory_planner_M->Timing.clockTick0;
23852 cartesian_trajectory_planner_M->Timing.t[0] = rtsiGetSolverStopTime
23853 (&cartesian_trajectory_planner_M->solverInfo);
23854
23855 {
23856 // Update absolute timer for sample time: [0.01s, 0.0s]
23857 // The "clockTick1" counts the number of times the code of this task has
23858 // been executed. The resolution of this integer timer is 0.01, which is the step size
23859 // of the task. Size of "clockTick1" ensures timer will not overflow during the
23860 // application lifespan selected.
23861
23862 cartesian_trajectory_planner_M->Timing.clockTick1++;
23863 }
23864 } // end MajorTimeStep
23865}
23866
23867// Derivatives for root system: '<Root>'
23868void cartesian_trajectory_planner_derivatives(void)
23869{
23870 int32_T i;
23871 XDot_cartesian_trajectory_pla_T *_rtXdot;
23872 _rtXdot = ((XDot_cartesian_trajectory_pla_T *)
23873 cartesian_trajectory_planner_M->derivs);
23874
23875 // Derivatives for Integrator: '<Root>/Integrator'
23876 for (i = 0; i < 6; i++) {
23877 _rtXdot->Integrator_CSTATE[i] =
23878 cartesian_trajectory_planner_B.MatrixMultiply1[i];
23879 }
23880
23881 // End of Derivatives for Integrator: '<Root>/Integrator'
23882}
23883
23884// Model initialize function
23885void cartesian_trajectory_planner_initialize(void)
23886{
23887 // Registration code
23888
23889 // initialize non-finites
23890 rt_InitInfAndNaN(sizeof(real_T));
23891
23892 {
23893 // Setup solver object
23894 rtsiSetSimTimeStepPtr(&cartesian_trajectory_planner_M->solverInfo,
23895 &cartesian_trajectory_planner_M->Timing.simTimeStep);
23896 rtsiSetTPtr(&cartesian_trajectory_planner_M->solverInfo, &rtmGetTPtr
23897 (cartesian_trajectory_planner_M));
23898 rtsiSetStepSizePtr(&cartesian_trajectory_planner_M->solverInfo,
23899 &cartesian_trajectory_planner_M->Timing.stepSize0);
23900 rtsiSetdXPtr(&cartesian_trajectory_planner_M->solverInfo,
23901 &cartesian_trajectory_planner_M->derivs);
23902 rtsiSetContStatesPtr(&cartesian_trajectory_planner_M->solverInfo, (real_T **)
23903 &cartesian_trajectory_planner_M->contStates);
23904 rtsiSetNumContStatesPtr(&cartesian_trajectory_planner_M->solverInfo,
23905 &cartesian_trajectory_planner_M->Sizes.numContStates);
23906 rtsiSetNumPeriodicContStatesPtr(&cartesian_trajectory_planner_M->solverInfo,
23907 &cartesian_trajectory_planner_M->Sizes.numPeriodicContStates);
23908 rtsiSetPeriodicContStateIndicesPtr
23909 (&cartesian_trajectory_planner_M->solverInfo,
23910 &cartesian_trajectory_planner_M->periodicContStateIndices);
23911 rtsiSetPeriodicContStateRangesPtr
23912 (&cartesian_trajectory_planner_M->solverInfo,
23913 &cartesian_trajectory_planner_M->periodicContStateRanges);
23914 rtsiSetErrorStatusPtr(&cartesian_trajectory_planner_M->solverInfo,
23915 (&rtmGetErrorStatus(cartesian_trajectory_planner_M)));
23916 rtsiSetRTModelPtr(&cartesian_trajectory_planner_M->solverInfo,
23917 cartesian_trajectory_planner_M);
23918 }
23919
23920 rtsiSetSimTimeStep(&cartesian_trajectory_planner_M->solverInfo,
23921 MAJOR_TIME_STEP);
23922 cartesian_trajectory_planner_M->intgData.y =
23923 cartesian_trajectory_planner_M->odeY;
23924 cartesian_trajectory_planner_M->intgData.f[0] =
23925 cartesian_trajectory_planner_M->odeF[0];
23926 cartesian_trajectory_planner_M->intgData.f[1] =
23927 cartesian_trajectory_planner_M->odeF[1];
23928 cartesian_trajectory_planner_M->intgData.f[2] =
23929 cartesian_trajectory_planner_M->odeF[2];
23930 cartesian_trajectory_planner_M->contStates = ((X_cartesian_trajectory_planne_T
23931 *) &cartesian_trajectory_planner_X);
23932 rtsiSetSolverData(&cartesian_trajectory_planner_M->solverInfo, static_cast<
23933 void *>(&cartesian_trajectory_planner_M->intgData));
23934 rtsiSetSolverName(&cartesian_trajectory_planner_M->solverInfo,"ode3");
23935 rtmSetTPtr(cartesian_trajectory_planner_M,
23936 &cartesian_trajectory_planner_M->Timing.tArray[0]);
23937 cartesian_trajectory_planner_M->Timing.stepSize0 = 0.01;
23938
23939 {
23940 robotics_slmanip_internal__as_T *obj;
23941 b_inverseKinematics_cartesian_T *obj_0;
23942 h_robotics_core_internal_Damp_T *obj_1;
23943 static const char_T tmp[6] = { '/', 'c', 'l', 'o', 'c', 'k' };
23944
23945 static const char_T tmp_0[17] = { '/', 'j', 'o', 'i', 'n', 't', '_', 't',
23946 'r', 'a', 'j', 'e', 'c', 't', 'o', 'r', 'y' };
23947
23948 static const char_T tmp_1[5] = { '/', 'o', 'i', '_', 'x' };
23949
23950 static const char_T tmp_2[5] = { '/', 'o', 'i', '_', 'y' };
23951
23952 static const char_T tmp_3[5] = { '/', 'o', 'i', '_', 'z' };
23953
23954 static const char_T tmp_4[5] = { '/', 'o', 'i', '_', 'w' };
23955
23956 static const char_T tmp_5[5] = { '/', 'p', 'i', '_', 'x' };
23957
23958 static const char_T tmp_6[5] = { '/', 'p', 'i', '_', 'y' };
23959
23960 static const char_T tmp_7[5] = { '/', 'p', 'i', '_', 'z' };
23961
23962 static const char_T tmp_8[5] = { '/', 'o', 'f', '_', 'x' };
23963
23964 static const char_T tmp_9[5] = { '/', 'o', 'f', '_', 'y' };
23965
23966 static const char_T tmp_a[5] = { '/', 'o', 'f', '_', 'z' };
23967
23968 static const char_T tmp_b[5] = { '/', 'o', 'f', '_', 'w' };
23969
23970 static const char_T tmp_c[5] = { '/', 'p', 'f', '_', 'x' };
23971
23972 static const char_T tmp_d[5] = { '/', 'p', 'f', '_', 'y' };
23973
23974 static const char_T tmp_e[5] = { '/', 'p', 'f', '_', 'z' };
23975
23976 static const char_T tmp_f[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23977 'o', 'x' };
23978
23979 static const char_T tmp_g[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23980 'o', 'y' };
23981
23982 static const char_T tmp_h[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23983 'o', 'z' };
23984
23985 static const char_T tmp_i[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23986 'p', 'x' };
23987
23988 static const char_T tmp_j[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23989 'p', 'y' };
23990
23991 static const char_T tmp_k[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
23992 'p', 'z' };
23993
23994 static const char_T tmp_l[12] = { '/', 's', 't', 'a', 'r', 't', '_', 'd',
23995 'e', 'l', 'a', 'y' };
23996
23997 // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe'
23998 // SystemInitialize for Enabled SubSystem: '<S10>/Enabled Subsystem'
23999 // SystemInitialize for Outport: '<S14>/Out1'
24000 cartesian_trajectory_planner_B.In1 = cartesian_trajectory_planner_P.Out1_Y0;
24001
24002 // End of SystemInitialize for SubSystem: '<S10>/Enabled Subsystem'
24003
24004 // Start for MATLABSystem: '<S10>/SourceBlock'
24005 cartesian_trajectory_planner_DW.obj_gl.matlabCodegenIsDeleted = true;
24006 cartesian_trajectory_planner_DW.obj_gl.isInitialized = 0;
24007 cartesian_trajectory_planner_DW.obj_gl.matlabCodegenIsDeleted = false;
24008 cartesian_trajectory_planner_DW.obj_gl.isSetupComplete = false;
24009 cartesian_trajectory_planner_DW.obj_gl.isInitialized = 1;
24010 for (cartesian_trajectory_planner_B.i_hp = 0;
24011 cartesian_trajectory_planner_B.i_hp < 6;
24012 cartesian_trajectory_planner_B.i_hp++) {
24013 // InitializeConditions for Delay: '<Root>/Delay'
24014 cartesian_trajectory_planner_DW.Delay_DSTATE[cartesian_trajectory_planner_B.i_hp]
24015 =
24016 cartesian_trajectory_planner_P.Delay_InitialCondition[cartesian_trajectory_planner_B.i_hp];
24017
24018 // InitializeConditions for Integrator: '<Root>/Integrator'
24019 cartesian_trajectory_planner_X.Integrator_CSTATE[cartesian_trajectory_planner_B.i_hp]
24020 = cartesian_trajectory_planner_P.Integrator_IC;
24021
24022 // Start for MATLABSystem: '<S10>/SourceBlock'
24023 cartesian_trajectory_planner_B.cv3[cartesian_trajectory_planner_B.i_hp] =
24024 tmp[cartesian_trajectory_planner_B.i_hp];
24025 }
24026
24027 // Start for MATLABSystem: '<S10>/SourceBlock'
24028 cartesian_trajectory_planner_B.cv3[6] = '\x00';
24029 Sub_cartesian_trajectory_planner_280.createSubscriber
24030 (cartesian_trajectory_planner_B.cv3, 1);
24031 cartesian_trajectory_planner_DW.obj_gl.isSetupComplete = true;
24032
24033 // End of SystemInitialize for SubSystem: '<Root>/Subscribe'
24034
24035 // SystemInitialize for Atomic SubSystem: '<Root>/Publish'
24036 // Start for MATLABSystem: '<S9>/SinkBlock'
24037 cartesian_trajectory_planner_DW.obj_n.matlabCodegenIsDeleted = true;
24038 cartesian_trajectory_planner_DW.obj_n.isInitialized = 0;
24039 cartesian_trajectory_planner_DW.obj_n.matlabCodegenIsDeleted = false;
24040 cartesian_trajectory_planner_DW.obj_n.isSetupComplete = false;
24041 cartesian_trajectory_planner_DW.obj_n.isInitialized = 1;
24042 for (cartesian_trajectory_planner_B.i_hp = 0;
24043 cartesian_trajectory_planner_B.i_hp < 17;
24044 cartesian_trajectory_planner_B.i_hp++) {
24045 cartesian_trajectory_planner_B.cv[cartesian_trajectory_planner_B.i_hp] =
24046 tmp_0[cartesian_trajectory_planner_B.i_hp];
24047 }
24048
24049 cartesian_trajectory_planner_B.cv[17] = '\x00';
24050 Pub_cartesian_trajectory_planner_278.createPublisher
24051 (cartesian_trajectory_planner_B.cv, 1);
24052 cartesian_trajectory_planner_DW.obj_n.isSetupComplete = true;
24053
24054 // End of Start for MATLABSystem: '<S9>/SinkBlock'
24055 // End of SystemInitialize for SubSystem: '<Root>/Publish'
24056
24057 // Start for MATLABSystem: '<Root>/Get Parameter'
24058 cartesian_trajectory_planner_DW.obj_cu.matlabCodegenIsDeleted = true;
24059 cartesian_trajectory_planner_DW.obj_cu.isInitialized = 0;
24060 cartesian_trajectory_planner_DW.obj_cu.matlabCodegenIsDeleted = false;
24061 cartesian_trajectory_planner_DW.obj_cu.isSetupComplete = false;
24062 cartesian_trajectory_planner_DW.obj_cu.isInitialized = 1;
24063 cartesian_trajectory_planner_B.cv5[0] = '/';
24064 cartesian_trajectory_planner_B.cv5[1] = 'V';
24065 cartesian_trajectory_planner_B.cv5[2] = 'e';
24066 cartesian_trajectory_planner_B.cv5[3] = 'e';
24067 cartesian_trajectory_planner_B.cv5[4] = '\x00';
24068 ParamGet_cartesian_trajectory_planner_271.initialize
24069 (cartesian_trajectory_planner_B.cv5);
24070 ParamGet_cartesian_trajectory_planner_271.initialize_error_codes(0, 1, 2, 3);
24071 ParamGet_cartesian_trajectory_planner_271.set_initial_value(1.0);
24072 cartesian_trajectory_planner_DW.obj_cu.isSetupComplete = true;
24073
24074 // Start for MATLABSystem: '<Root>/Get Parameter1'
24075 cartesian_trajectory_planner_DW.obj_k.matlabCodegenIsDeleted = true;
24076 cartesian_trajectory_planner_DW.obj_k.isInitialized = 0;
24077 cartesian_trajectory_planner_DW.obj_k.matlabCodegenIsDeleted = false;
24078 cartesian_trajectory_planner_DW.obj_k.isSetupComplete = false;
24079 cartesian_trajectory_planner_DW.obj_k.isInitialized = 1;
24080 cartesian_trajectory_planner_B.cv5[0] = '/';
24081 cartesian_trajectory_planner_B.cv5[1] = 'A';
24082 cartesian_trajectory_planner_B.cv5[2] = 'e';
24083 cartesian_trajectory_planner_B.cv5[3] = 'e';
24084 cartesian_trajectory_planner_B.cv5[4] = '\x00';
24085 ParamGet_cartesian_trajectory_planner_272.initialize
24086 (cartesian_trajectory_planner_B.cv5);
24087 ParamGet_cartesian_trajectory_planner_272.initialize_error_codes(0, 1, 2, 3);
24088 ParamGet_cartesian_trajectory_planner_272.set_initial_value(0.1);
24089 cartesian_trajectory_planner_DW.obj_k.isSetupComplete = true;
24090
24091 // Start for MATLABSystem: '<S15>/Get Parameter3'
24092 cartesian_trajectory_planner_DW.obj_es.matlabCodegenIsDeleted = true;
24093 cartesian_trajectory_planner_DW.obj_es.isInitialized = 0;
24094 cartesian_trajectory_planner_DW.obj_es.matlabCodegenIsDeleted = false;
24095 cartesian_trajectory_planner_DW.obj_es.isSetupComplete = false;
24096 cartesian_trajectory_planner_DW.obj_es.isInitialized = 1;
24097 for (cartesian_trajectory_planner_B.i_hp = 0;
24098 cartesian_trajectory_planner_B.i_hp < 5;
24099 cartesian_trajectory_planner_B.i_hp++) {
24100 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24101 tmp_1[cartesian_trajectory_planner_B.i_hp];
24102 }
24103
24104 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24105 ParamGet_cartesian_trajectory_planner_291.initialize
24106 (cartesian_trajectory_planner_B.cv4);
24107 ParamGet_cartesian_trajectory_planner_291.initialize_error_codes(0, 1, 2, 3);
24108 ParamGet_cartesian_trajectory_planner_291.set_initial_value(0.0);
24109 cartesian_trajectory_planner_DW.obj_es.isSetupComplete = true;
24110
24111 // End of Start for MATLABSystem: '<S15>/Get Parameter3'
24112
24113 // Start for MATLABSystem: '<S15>/Get Parameter4'
24114 cartesian_trajectory_planner_DW.obj_f4.matlabCodegenIsDeleted = true;
24115 cartesian_trajectory_planner_DW.obj_f4.isInitialized = 0;
24116 cartesian_trajectory_planner_DW.obj_f4.matlabCodegenIsDeleted = false;
24117 cartesian_trajectory_planner_DW.obj_f4.isSetupComplete = false;
24118 cartesian_trajectory_planner_DW.obj_f4.isInitialized = 1;
24119 for (cartesian_trajectory_planner_B.i_hp = 0;
24120 cartesian_trajectory_planner_B.i_hp < 5;
24121 cartesian_trajectory_planner_B.i_hp++) {
24122 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24123 tmp_2[cartesian_trajectory_planner_B.i_hp];
24124 }
24125
24126 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24127 ParamGet_cartesian_trajectory_planner_292.initialize
24128 (cartesian_trajectory_planner_B.cv4);
24129 ParamGet_cartesian_trajectory_planner_292.initialize_error_codes(0, 1, 2, 3);
24130 ParamGet_cartesian_trajectory_planner_292.set_initial_value(0.0);
24131 cartesian_trajectory_planner_DW.obj_f4.isSetupComplete = true;
24132
24133 // End of Start for MATLABSystem: '<S15>/Get Parameter4'
24134
24135 // Start for MATLABSystem: '<S15>/Get Parameter5'
24136 cartesian_trajectory_planner_DW.obj_en.matlabCodegenIsDeleted = true;
24137 cartesian_trajectory_planner_DW.obj_en.isInitialized = 0;
24138 cartesian_trajectory_planner_DW.obj_en.matlabCodegenIsDeleted = false;
24139 cartesian_trajectory_planner_DW.obj_en.isSetupComplete = false;
24140 cartesian_trajectory_planner_DW.obj_en.isInitialized = 1;
24141 for (cartesian_trajectory_planner_B.i_hp = 0;
24142 cartesian_trajectory_planner_B.i_hp < 5;
24143 cartesian_trajectory_planner_B.i_hp++) {
24144 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24145 tmp_3[cartesian_trajectory_planner_B.i_hp];
24146 }
24147
24148 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24149 ParamGet_cartesian_trajectory_planner_293.initialize
24150 (cartesian_trajectory_planner_B.cv4);
24151 ParamGet_cartesian_trajectory_planner_293.initialize_error_codes(0, 1, 2, 3);
24152 ParamGet_cartesian_trajectory_planner_293.set_initial_value(0.0);
24153 cartesian_trajectory_planner_DW.obj_en.isSetupComplete = true;
24154
24155 // End of Start for MATLABSystem: '<S15>/Get Parameter5'
24156
24157 // Start for MATLABSystem: '<S15>/Get Parameter6'
24158 cartesian_trajectory_planner_DW.obj_l0.matlabCodegenIsDeleted = true;
24159 cartesian_trajectory_planner_DW.obj_l0.isInitialized = 0;
24160 cartesian_trajectory_planner_DW.obj_l0.matlabCodegenIsDeleted = false;
24161 cartesian_trajectory_planner_DW.obj_l0.isSetupComplete = false;
24162 cartesian_trajectory_planner_DW.obj_l0.isInitialized = 1;
24163 for (cartesian_trajectory_planner_B.i_hp = 0;
24164 cartesian_trajectory_planner_B.i_hp < 5;
24165 cartesian_trajectory_planner_B.i_hp++) {
24166 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24167 tmp_4[cartesian_trajectory_planner_B.i_hp];
24168 }
24169
24170 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24171 ParamGet_cartesian_trajectory_planner_294.initialize
24172 (cartesian_trajectory_planner_B.cv4);
24173 ParamGet_cartesian_trajectory_planner_294.initialize_error_codes(0, 1, 2, 3);
24174 ParamGet_cartesian_trajectory_planner_294.set_initial_value(1.0);
24175 cartesian_trajectory_planner_DW.obj_l0.isSetupComplete = true;
24176
24177 // End of Start for MATLABSystem: '<S15>/Get Parameter6'
24178
24179 // Start for MATLABSystem: '<S15>/Get Parameter'
24180 cartesian_trajectory_planner_DW.obj_i.matlabCodegenIsDeleted = true;
24181 cartesian_trajectory_planner_DW.obj_i.isInitialized = 0;
24182 cartesian_trajectory_planner_DW.obj_i.matlabCodegenIsDeleted = false;
24183 cartesian_trajectory_planner_DW.obj_i.isSetupComplete = false;
24184 cartesian_trajectory_planner_DW.obj_i.isInitialized = 1;
24185 for (cartesian_trajectory_planner_B.i_hp = 0;
24186 cartesian_trajectory_planner_B.i_hp < 5;
24187 cartesian_trajectory_planner_B.i_hp++) {
24188 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24189 tmp_5[cartesian_trajectory_planner_B.i_hp];
24190 }
24191
24192 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24193 ParamGet_cartesian_trajectory_planner_288.initialize
24194 (cartesian_trajectory_planner_B.cv4);
24195 ParamGet_cartesian_trajectory_planner_288.initialize_error_codes(0, 1, 2, 3);
24196 ParamGet_cartesian_trajectory_planner_288.set_initial_value(0.0);
24197 cartesian_trajectory_planner_DW.obj_i.isSetupComplete = true;
24198
24199 // End of Start for MATLABSystem: '<S15>/Get Parameter'
24200
24201 // Start for MATLABSystem: '<S15>/Get Parameter1'
24202 cartesian_trajectory_planner_DW.obj_db.matlabCodegenIsDeleted = true;
24203 cartesian_trajectory_planner_DW.obj_db.isInitialized = 0;
24204 cartesian_trajectory_planner_DW.obj_db.matlabCodegenIsDeleted = false;
24205 cartesian_trajectory_planner_DW.obj_db.isSetupComplete = false;
24206 cartesian_trajectory_planner_DW.obj_db.isInitialized = 1;
24207 for (cartesian_trajectory_planner_B.i_hp = 0;
24208 cartesian_trajectory_planner_B.i_hp < 5;
24209 cartesian_trajectory_planner_B.i_hp++) {
24210 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24211 tmp_6[cartesian_trajectory_planner_B.i_hp];
24212 }
24213
24214 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24215 ParamGet_cartesian_trajectory_planner_289.initialize
24216 (cartesian_trajectory_planner_B.cv4);
24217 ParamGet_cartesian_trajectory_planner_289.initialize_error_codes(0, 1, 2, 3);
24218 ParamGet_cartesian_trajectory_planner_289.set_initial_value(0.0);
24219 cartesian_trajectory_planner_DW.obj_db.isSetupComplete = true;
24220
24221 // End of Start for MATLABSystem: '<S15>/Get Parameter1'
24222
24223 // Start for MATLABSystem: '<S15>/Get Parameter2'
24224 cartesian_trajectory_planner_DW.obj_oq.matlabCodegenIsDeleted = true;
24225 cartesian_trajectory_planner_DW.obj_oq.isInitialized = 0;
24226 cartesian_trajectory_planner_DW.obj_oq.matlabCodegenIsDeleted = false;
24227 cartesian_trajectory_planner_DW.obj_oq.isSetupComplete = false;
24228 cartesian_trajectory_planner_DW.obj_oq.isInitialized = 1;
24229 for (cartesian_trajectory_planner_B.i_hp = 0;
24230 cartesian_trajectory_planner_B.i_hp < 5;
24231 cartesian_trajectory_planner_B.i_hp++) {
24232 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24233 tmp_7[cartesian_trajectory_planner_B.i_hp];
24234 }
24235
24236 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24237 ParamGet_cartesian_trajectory_planner_290.initialize
24238 (cartesian_trajectory_planner_B.cv4);
24239 ParamGet_cartesian_trajectory_planner_290.initialize_error_codes(0, 1, 2, 3);
24240 ParamGet_cartesian_trajectory_planner_290.set_initial_value(0.99);
24241 cartesian_trajectory_planner_DW.obj_oq.isSetupComplete = true;
24242
24243 // End of Start for MATLABSystem: '<S15>/Get Parameter2'
24244
24245 // Start for MATLABSystem: '<S16>/Get Parameter3'
24246 cartesian_trajectory_planner_DW.obj_e0.matlabCodegenIsDeleted = true;
24247 cartesian_trajectory_planner_DW.obj_e0.isInitialized = 0;
24248 cartesian_trajectory_planner_DW.obj_e0.matlabCodegenIsDeleted = false;
24249 cartesian_trajectory_planner_DW.obj_e0.isSetupComplete = false;
24250 cartesian_trajectory_planner_DW.obj_e0.isInitialized = 1;
24251 for (cartesian_trajectory_planner_B.i_hp = 0;
24252 cartesian_trajectory_planner_B.i_hp < 5;
24253 cartesian_trajectory_planner_B.i_hp++) {
24254 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24255 tmp_8[cartesian_trajectory_planner_B.i_hp];
24256 }
24257
24258 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24259 ParamGet_cartesian_trajectory_planner_303.initialize
24260 (cartesian_trajectory_planner_B.cv4);
24261 ParamGet_cartesian_trajectory_planner_303.initialize_error_codes(0, 1, 2, 3);
24262 ParamGet_cartesian_trajectory_planner_303.set_initial_value(0.39);
24263 cartesian_trajectory_planner_DW.obj_e0.isSetupComplete = true;
24264
24265 // End of Start for MATLABSystem: '<S16>/Get Parameter3'
24266
24267 // Start for MATLABSystem: '<S16>/Get Parameter4'
24268 cartesian_trajectory_planner_DW.obj_a.matlabCodegenIsDeleted = true;
24269 cartesian_trajectory_planner_DW.obj_a.isInitialized = 0;
24270 cartesian_trajectory_planner_DW.obj_a.matlabCodegenIsDeleted = false;
24271 cartesian_trajectory_planner_DW.obj_a.isSetupComplete = false;
24272 cartesian_trajectory_planner_DW.obj_a.isInitialized = 1;
24273 for (cartesian_trajectory_planner_B.i_hp = 0;
24274 cartesian_trajectory_planner_B.i_hp < 5;
24275 cartesian_trajectory_planner_B.i_hp++) {
24276 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24277 tmp_9[cartesian_trajectory_planner_B.i_hp];
24278 }
24279
24280 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24281 ParamGet_cartesian_trajectory_planner_304.initialize
24282 (cartesian_trajectory_planner_B.cv4);
24283 ParamGet_cartesian_trajectory_planner_304.initialize_error_codes(0, 1, 2, 3);
24284 ParamGet_cartesian_trajectory_planner_304.set_initial_value(0.89);
24285 cartesian_trajectory_planner_DW.obj_a.isSetupComplete = true;
24286
24287 // End of Start for MATLABSystem: '<S16>/Get Parameter4'
24288
24289 // Start for MATLABSystem: '<S16>/Get Parameter5'
24290 cartesian_trajectory_planner_DW.obj_p.matlabCodegenIsDeleted = true;
24291 cartesian_trajectory_planner_DW.obj_p.isInitialized = 0;
24292 cartesian_trajectory_planner_DW.obj_p.matlabCodegenIsDeleted = false;
24293 cartesian_trajectory_planner_DW.obj_p.isSetupComplete = false;
24294 cartesian_trajectory_planner_DW.obj_p.isInitialized = 1;
24295 for (cartesian_trajectory_planner_B.i_hp = 0;
24296 cartesian_trajectory_planner_B.i_hp < 5;
24297 cartesian_trajectory_planner_B.i_hp++) {
24298 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24299 tmp_a[cartesian_trajectory_planner_B.i_hp];
24300 }
24301
24302 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24303 ParamGet_cartesian_trajectory_planner_305.initialize
24304 (cartesian_trajectory_planner_B.cv4);
24305 ParamGet_cartesian_trajectory_planner_305.initialize_error_codes(0, 1, 2, 3);
24306 ParamGet_cartesian_trajectory_planner_305.set_initial_value(0.2);
24307 cartesian_trajectory_planner_DW.obj_p.isSetupComplete = true;
24308
24309 // End of Start for MATLABSystem: '<S16>/Get Parameter5'
24310
24311 // Start for MATLABSystem: '<S16>/Get Parameter6'
24312 cartesian_trajectory_planner_DW.obj_l.matlabCodegenIsDeleted = true;
24313 cartesian_trajectory_planner_DW.obj_l.isInitialized = 0;
24314 cartesian_trajectory_planner_DW.obj_l.matlabCodegenIsDeleted = false;
24315 cartesian_trajectory_planner_DW.obj_l.isSetupComplete = false;
24316 cartesian_trajectory_planner_DW.obj_l.isInitialized = 1;
24317 for (cartesian_trajectory_planner_B.i_hp = 0;
24318 cartesian_trajectory_planner_B.i_hp < 5;
24319 cartesian_trajectory_planner_B.i_hp++) {
24320 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24321 tmp_b[cartesian_trajectory_planner_B.i_hp];
24322 }
24323
24324 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24325 ParamGet_cartesian_trajectory_planner_306.initialize
24326 (cartesian_trajectory_planner_B.cv4);
24327 ParamGet_cartesian_trajectory_planner_306.initialize_error_codes(0, 1, 2, 3);
24328 ParamGet_cartesian_trajectory_planner_306.set_initial_value(-0.12);
24329 cartesian_trajectory_planner_DW.obj_l.isSetupComplete = true;
24330
24331 // End of Start for MATLABSystem: '<S16>/Get Parameter6'
24332
24333 // Start for MATLABSystem: '<S16>/Get Parameter'
24334 cartesian_trajectory_planner_DW.obj_mb.matlabCodegenIsDeleted = true;
24335 cartesian_trajectory_planner_DW.obj_mb.isInitialized = 0;
24336 cartesian_trajectory_planner_DW.obj_mb.matlabCodegenIsDeleted = false;
24337 cartesian_trajectory_planner_DW.obj_mb.isSetupComplete = false;
24338 cartesian_trajectory_planner_DW.obj_mb.isInitialized = 1;
24339 for (cartesian_trajectory_planner_B.i_hp = 0;
24340 cartesian_trajectory_planner_B.i_hp < 5;
24341 cartesian_trajectory_planner_B.i_hp++) {
24342 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24343 tmp_c[cartesian_trajectory_planner_B.i_hp];
24344 }
24345
24346 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24347 ParamGet_cartesian_trajectory_planner_300.initialize
24348 (cartesian_trajectory_planner_B.cv4);
24349 ParamGet_cartesian_trajectory_planner_300.initialize_error_codes(0, 1, 2, 3);
24350 ParamGet_cartesian_trajectory_planner_300.set_initial_value(0.3);
24351 cartesian_trajectory_planner_DW.obj_mb.isSetupComplete = true;
24352
24353 // End of Start for MATLABSystem: '<S16>/Get Parameter'
24354
24355 // Start for MATLABSystem: '<S16>/Get Parameter1'
24356 cartesian_trajectory_planner_DW.obj_c.matlabCodegenIsDeleted = true;
24357 cartesian_trajectory_planner_DW.obj_c.isInitialized = 0;
24358 cartesian_trajectory_planner_DW.obj_c.matlabCodegenIsDeleted = false;
24359 cartesian_trajectory_planner_DW.obj_c.isSetupComplete = false;
24360 cartesian_trajectory_planner_DW.obj_c.isInitialized = 1;
24361 for (cartesian_trajectory_planner_B.i_hp = 0;
24362 cartesian_trajectory_planner_B.i_hp < 5;
24363 cartesian_trajectory_planner_B.i_hp++) {
24364 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24365 tmp_d[cartesian_trajectory_planner_B.i_hp];
24366 }
24367
24368 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24369 ParamGet_cartesian_trajectory_planner_301.initialize
24370 (cartesian_trajectory_planner_B.cv4);
24371 ParamGet_cartesian_trajectory_planner_301.initialize_error_codes(0, 1, 2, 3);
24372 ParamGet_cartesian_trajectory_planner_301.set_initial_value(0.3);
24373 cartesian_trajectory_planner_DW.obj_c.isSetupComplete = true;
24374
24375 // End of Start for MATLABSystem: '<S16>/Get Parameter1'
24376
24377 // Start for MATLABSystem: '<S16>/Get Parameter2'
24378 cartesian_trajectory_planner_DW.obj_f.matlabCodegenIsDeleted = true;
24379 cartesian_trajectory_planner_DW.obj_f.isInitialized = 0;
24380 cartesian_trajectory_planner_DW.obj_f.matlabCodegenIsDeleted = false;
24381 cartesian_trajectory_planner_DW.obj_f.isSetupComplete = false;
24382 cartesian_trajectory_planner_DW.obj_f.isInitialized = 1;
24383 for (cartesian_trajectory_planner_B.i_hp = 0;
24384 cartesian_trajectory_planner_B.i_hp < 5;
24385 cartesian_trajectory_planner_B.i_hp++) {
24386 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_hp] =
24387 tmp_e[cartesian_trajectory_planner_B.i_hp];
24388 }
24389
24390 cartesian_trajectory_planner_B.cv4[5] = '\x00';
24391 ParamGet_cartesian_trajectory_planner_302.initialize
24392 (cartesian_trajectory_planner_B.cv4);
24393 ParamGet_cartesian_trajectory_planner_302.initialize_error_codes(0, 1, 2, 3);
24394 ParamGet_cartesian_trajectory_planner_302.set_initial_value(0.3);
24395 cartesian_trajectory_planner_DW.obj_f.isSetupComplete = true;
24396
24397 // End of Start for MATLABSystem: '<S16>/Get Parameter2'
24398
24399 // Start for MATLABSystem: '<S12>/Get Parameter'
24400 cartesian_trajectory_planner_DW.obj_e.matlabCodegenIsDeleted = true;
24401 cartesian_trajectory_planner_DW.obj_e.isInitialized = 0;
24402 cartesian_trajectory_planner_DW.obj_e.matlabCodegenIsDeleted = false;
24403 cartesian_trajectory_planner_DW.obj_e.isSetupComplete = false;
24404 cartesian_trajectory_planner_DW.obj_e.isInitialized = 1;
24405 for (cartesian_trajectory_planner_B.i_hp = 0;
24406 cartesian_trajectory_planner_B.i_hp < 10;
24407 cartesian_trajectory_planner_B.i_hp++) {
24408 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_hp] =
24409 tmp_f[cartesian_trajectory_planner_B.i_hp];
24410 }
24411
24412 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24413 ParamGet_cartesian_trajectory_planner_312.initialize
24414 (cartesian_trajectory_planner_B.cv2);
24415 ParamGet_cartesian_trajectory_planner_312.initialize_error_codes(0, 1, 2, 3);
24416 ParamGet_cartesian_trajectory_planner_312.set_initial_value(0.0);
24417 cartesian_trajectory_planner_DW.obj_e.isSetupComplete = true;
24418
24419 // End of Start for MATLABSystem: '<S12>/Get Parameter'
24420
24421 // Start for MATLABSystem: '<S12>/Get Parameter1'
24422 cartesian_trajectory_planner_DW.obj_d.matlabCodegenIsDeleted = true;
24423 cartesian_trajectory_planner_DW.obj_d.isInitialized = 0;
24424 cartesian_trajectory_planner_DW.obj_d.matlabCodegenIsDeleted = false;
24425 cartesian_trajectory_planner_DW.obj_d.isSetupComplete = false;
24426 cartesian_trajectory_planner_DW.obj_d.isInitialized = 1;
24427 for (cartesian_trajectory_planner_B.i_hp = 0;
24428 cartesian_trajectory_planner_B.i_hp < 10;
24429 cartesian_trajectory_planner_B.i_hp++) {
24430 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_hp] =
24431 tmp_g[cartesian_trajectory_planner_B.i_hp];
24432 }
24433
24434 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24435 ParamGet_cartesian_trajectory_planner_313.initialize
24436 (cartesian_trajectory_planner_B.cv2);
24437 ParamGet_cartesian_trajectory_planner_313.initialize_error_codes(0, 1, 2, 3);
24438 ParamGet_cartesian_trajectory_planner_313.set_initial_value(0.0);
24439 cartesian_trajectory_planner_DW.obj_d.isSetupComplete = true;
24440
24441 // End of Start for MATLABSystem: '<S12>/Get Parameter1'
24442
24443 // Start for MATLABSystem: '<S12>/Get Parameter2'
24444 cartesian_trajectory_planner_DW.obj_m.matlabCodegenIsDeleted = true;
24445 cartesian_trajectory_planner_DW.obj_m.isInitialized = 0;
24446 cartesian_trajectory_planner_DW.obj_m.matlabCodegenIsDeleted = false;
24447 cartesian_trajectory_planner_DW.obj_m.isSetupComplete = false;
24448 cartesian_trajectory_planner_DW.obj_m.isInitialized = 1;
24449 for (cartesian_trajectory_planner_B.i_hp = 0;
24450 cartesian_trajectory_planner_B.i_hp < 10;
24451 cartesian_trajectory_planner_B.i_hp++) {
24452 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_hp] =
24453 tmp_h[cartesian_trajectory_planner_B.i_hp];
24454 }
24455
24456 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24457 ParamGet_cartesian_trajectory_planner_314.initialize
24458 (cartesian_trajectory_planner_B.cv2);
24459 ParamGet_cartesian_trajectory_planner_314.initialize_error_codes(0, 1, 2, 3);
24460 ParamGet_cartesian_trajectory_planner_314.set_initial_value(0.0);
24461 cartesian_trajectory_planner_DW.obj_m.isSetupComplete = true;
24462
24463 // End of Start for MATLABSystem: '<S12>/Get Parameter2'
24464
24465 // Start for MATLABSystem: '<S12>/Get Parameter3'
24466 cartesian_trajectory_planner_DW.obj_h.matlabCodegenIsDeleted = true;
24467 cartesian_trajectory_planner_DW.obj_h.isInitialized = 0;
24468 cartesian_trajectory_planner_DW.obj_h.matlabCodegenIsDeleted = false;
24469 cartesian_trajectory_planner_DW.obj_h.isSetupComplete = false;
24470 cartesian_trajectory_planner_DW.obj_h.isInitialized = 1;
24471 for (cartesian_trajectory_planner_B.i_hp = 0;
24472 cartesian_trajectory_planner_B.i_hp < 10;
24473 cartesian_trajectory_planner_B.i_hp++) {
24474 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_hp] =
24475 tmp_i[cartesian_trajectory_planner_B.i_hp];
24476 }
24477
24478 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24479 ParamGet_cartesian_trajectory_planner_315.initialize
24480 (cartesian_trajectory_planner_B.cv2);
24481 ParamGet_cartesian_trajectory_planner_315.initialize_error_codes(0, 1, 2, 3);
24482 ParamGet_cartesian_trajectory_planner_315.set_initial_value(1.0);
24483 cartesian_trajectory_planner_DW.obj_h.isSetupComplete = true;
24484
24485 // End of Start for MATLABSystem: '<S12>/Get Parameter3'
24486
24487 // Start for MATLABSystem: '<S12>/Get Parameter4'
24488 cartesian_trajectory_planner_DW.obj_mf.matlabCodegenIsDeleted = true;
24489 cartesian_trajectory_planner_DW.obj_mf.isInitialized = 0;
24490 cartesian_trajectory_planner_DW.obj_mf.matlabCodegenIsDeleted = false;
24491 cartesian_trajectory_planner_DW.obj_mf.isSetupComplete = false;
24492 cartesian_trajectory_planner_DW.obj_mf.isInitialized = 1;
24493 for (cartesian_trajectory_planner_B.i_hp = 0;
24494 cartesian_trajectory_planner_B.i_hp < 10;
24495 cartesian_trajectory_planner_B.i_hp++) {
24496 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_hp] =
24497 tmp_j[cartesian_trajectory_planner_B.i_hp];
24498 }
24499
24500 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24501 ParamGet_cartesian_trajectory_planner_316.initialize
24502 (cartesian_trajectory_planner_B.cv2);
24503 ParamGet_cartesian_trajectory_planner_316.initialize_error_codes(0, 1, 2, 3);
24504 ParamGet_cartesian_trajectory_planner_316.set_initial_value(1.0);
24505 cartesian_trajectory_planner_DW.obj_mf.isSetupComplete = true;
24506
24507 // End of Start for MATLABSystem: '<S12>/Get Parameter4'
24508
24509 // Start for MATLABSystem: '<S12>/Get Parameter5'
24510 cartesian_trajectory_planner_DW.obj_o.matlabCodegenIsDeleted = true;
24511 cartesian_trajectory_planner_DW.obj_o.isInitialized = 0;
24512 cartesian_trajectory_planner_DW.obj_o.matlabCodegenIsDeleted = false;
24513 cartesian_trajectory_planner_DW.obj_o.isSetupComplete = false;
24514 cartesian_trajectory_planner_DW.obj_o.isInitialized = 1;
24515 for (cartesian_trajectory_planner_B.i_hp = 0;
24516 cartesian_trajectory_planner_B.i_hp < 10;
24517 cartesian_trajectory_planner_B.i_hp++) {
24518 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_hp] =
24519 tmp_k[cartesian_trajectory_planner_B.i_hp];
24520 }
24521
24522 cartesian_trajectory_planner_B.cv2[10] = '\x00';
24523 ParamGet_cartesian_trajectory_planner_317.initialize
24524 (cartesian_trajectory_planner_B.cv2);
24525 ParamGet_cartesian_trajectory_planner_317.initialize_error_codes(0, 1, 2, 3);
24526 ParamGet_cartesian_trajectory_planner_317.set_initial_value(1.0);
24527 cartesian_trajectory_planner_DW.obj_o.isSetupComplete = true;
24528
24529 // End of Start for MATLABSystem: '<S12>/Get Parameter5'
24530 emxInitStruct_robotics_slmani_a(&cartesian_trajectory_planner_DW.obj);
24531 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_1);
24532 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_50);
24533 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_49);
24534 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_48);
24535 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_47);
24536 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_46);
24537 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_45);
24538 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_44);
24539 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_43);
24540 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_42);
24541 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_41);
24542 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_40);
24543 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_39);
24544 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_38);
24545 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_37);
24546 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_36);
24547 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_35);
24548 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_34);
24549 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_33);
24550 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_32);
24551 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_31);
24552 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_30);
24553 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_29);
24554 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_28);
24555 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_27);
24556 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_26);
24557 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_25);
24558 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_24);
24559 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_23);
24560 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_22);
24561 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_21);
24562 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_20);
24563 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_19);
24564 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_18);
24565 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_17);
24566 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_16);
24567 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_15);
24568 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_14);
24569 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_13);
24570 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_12);
24571 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_11);
24572 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_10);
24573 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_9);
24574 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_8);
24575 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_7);
24576 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_6);
24577 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_5);
24578 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_4);
24579 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_3);
24580 emxInitStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_2);
24581 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_51);
24582 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_82);
24583 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_81);
24584 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_80);
24585 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_79);
24586 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_78);
24587 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_77);
24588 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_76);
24589 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_75);
24590 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_74);
24591 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_73);
24592 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_72);
24593 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_71);
24594 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_70);
24595 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_69);
24596 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_68);
24597 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_67);
24598 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_66);
24599 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_65);
24600 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_64);
24601 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_63);
24602 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_62);
24603 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_61);
24604 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_60);
24605 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_59);
24606 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_58);
24607 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_57);
24608 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_56);
24609 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_55);
24610 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_54);
24611 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_53);
24612 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_52);
24613 emxInitStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_83);
24614 emxInitStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_84);
24615 emxInitStruct_f_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_85);
24616 emxInitStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_86);
24617 emxInitStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_87);
24618 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_88);
24619 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_103);
24620 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_102);
24621 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_101);
24622 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_100);
24623 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_99);
24624 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_98);
24625 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_97);
24626 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_96);
24627 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_95);
24628 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_94);
24629 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_93);
24630 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_92);
24631 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_91);
24632 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_90);
24633 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_89);
24634
24635 // Start for MATLABSystem: '<S6>/MATLAB System'
24636 cartesian_trajectory_planner_DW.obj.IKInternal.matlabCodegenIsDeleted = true;
24637 cartesian_trajectory_planner_DW.obj.matlabCodegenIsDeleted = true;
24638 cartesian_tr_eml_rand_mt19937ar(cartesian_trajectory_planner_DW.state_m);
24639 cartesian_trajectory_planner_DW.obj.isInitialized = 0;
24640 cartesian_trajectory_planner_DW.obj.matlabCodegenIsDeleted = false;
24641 obj = &cartesian_trajectory_planner_DW.obj;
24642 cartesian_trajectory_planner_DW.obj.isInitialized = 1;
24643 RigidBodyTree_RigidBodyTree_as
24644 (&cartesian_trajectory_planner_DW.obj.TreeInternal,
24645 &cartesian_trajectory_planner_DW.gobj_90,
24646 &cartesian_trajectory_planner_DW.gobj_91,
24647 &cartesian_trajectory_planner_DW.gobj_92,
24648 &cartesian_trajectory_planner_DW.gobj_93,
24649 &cartesian_trajectory_planner_DW.gobj_94,
24650 &cartesian_trajectory_planner_DW.gobj_95,
24651 &cartesian_trajectory_planner_DW.gobj_96,
24652 &cartesian_trajectory_planner_DW.gobj_89);
24653 obj_0 = &cartesian_trajectory_planner_DW.obj.IKInternal;
24654 obj_0->isInitialized = 0;
24655 inverseKinematics_set_RigidBody(obj_0, &obj->TreeInternal,
24656 &cartesian_trajectory_planner_DW.gobj_69,
24657 &cartesian_trajectory_planner_DW.gobj_70,
24658 &cartesian_trajectory_planner_DW.gobj_71,
24659 &cartesian_trajectory_planner_DW.gobj_72,
24660 &cartesian_trajectory_planner_DW.gobj_73,
24661 &cartesian_trajectory_planner_DW.gobj_74,
24662 &cartesian_trajectory_planner_DW.gobj_75,
24663 &cartesian_trajectory_planner_DW.gobj_76,
24664 &cartesian_trajectory_planner_DW.gobj_77,
24665 &cartesian_trajectory_planner_DW.gobj_78,
24666 &cartesian_trajectory_planner_DW.gobj_79,
24667 &cartesian_trajectory_planner_DW.gobj_80,
24668 &cartesian_trajectory_planner_DW.gobj_81,
24669 &cartesian_trajectory_planner_DW.gobj_82,
24670 &cartesian_trajectory_planner_DW.gobj_51,
24671 &cartesian_trajectory_planner_DW.gobj_28,
24672 &cartesian_trajectory_planner_DW.gobj_29,
24673 &cartesian_trajectory_planner_DW.gobj_30,
24674 &cartesian_trajectory_planner_DW.gobj_31,
24675 &cartesian_trajectory_planner_DW.gobj_32,
24676 &cartesian_trajectory_planner_DW.gobj_33,
24677 &cartesian_trajectory_planner_DW.gobj_34,
24678 &cartesian_trajectory_planner_DW.gobj_35,
24679 &cartesian_trajectory_planner_DW.gobj_36,
24680 &cartesian_trajectory_planner_DW.gobj_37,
24681 &cartesian_trajectory_planner_DW.gobj_38,
24682 &cartesian_trajectory_planner_DW.gobj_39,
24683 &cartesian_trajectory_planner_DW.gobj_40,
24684 &cartesian_trajectory_planner_DW.gobj_41,
24685 &cartesian_trajectory_planner_DW.gobj_42,
24686 &cartesian_trajectory_planner_DW.gobj_43,
24687 &cartesian_trajectory_planner_DW.gobj_44,
24688 &cartesian_trajectory_planner_DW.gobj_45,
24689 &cartesian_trajectory_planner_DW.gobj_46,
24690 &cartesian_trajectory_planner_DW.gobj_47,
24691 &cartesian_trajectory_planner_DW.gobj_48,
24692 &cartesian_trajectory_planner_DW.gobj_49,
24693 &cartesian_trajectory_planner_DW.gobj_50,
24694 &cartesian_trajectory_planner_DW.gobj_1,
24695 &cartesian_trajectory_planner_DW.gobj_27,
24696 &cartesian_trajectory_planner_DW.gobj_68,
24697 &cartesian_trajectory_planner_DW.gobj_83);
24698 obj_0->Solver = DampedBFGSwGradientProjection_D
24699 (&cartesian_trajectory_planner_DW.gobj_87);
24700 obj_1 = obj_0->Solver;
24701 obj_1->MaxNumIteration = 1500.0;
24702 obj_1->MaxTime = 10.0;
24703 obj_1->GradientTolerance = 1.0E-7;
24704 obj_1->SolutionTolerance = 1.0E-6;
24705 obj_1->ConstraintsOn = true;
24706 obj_1->RandomRestart = false;
24707 obj_1->StepTolerance = 1.0E-14;
24708 obj_0->matlabCodegenIsDeleted = false;
24709 emxInitStruct_robotics_slman_as(&cartesian_trajectory_planner_DW.obj_g);
24710 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_1_h);
24711 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_16_c);
24712 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_15_g);
24713 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_14_h);
24714 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_13_a);
24715 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_12_b);
24716 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_11_i);
24717 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_10_f);
24718 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_9_g);
24719 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_8_d);
24720 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_7_b);
24721 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_6_a);
24722 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_5_g);
24723 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_4_j);
24724 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_3_l);
24725 emxInitStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_2_m);
24726
24727 // Start for MATLABSystem: '<S3>/MATLAB System'
24728 cartesian_trajectory_planner_DW.obj_g.isInitialized = 0;
24729 cartesian_trajectory_planner_DW.obj_g.isInitialized = 1;
24730 c_RigidBodyTree_RigidBodyTree_a
24731 (&cartesian_trajectory_planner_DW.obj_g.TreeInternal,
24732 &cartesian_trajectory_planner_DW.gobj_2_m,
24733 &cartesian_trajectory_planner_DW.gobj_4_j,
24734 &cartesian_trajectory_planner_DW.gobj_5_g,
24735 &cartesian_trajectory_planner_DW.gobj_6_a,
24736 &cartesian_trajectory_planner_DW.gobj_7_b,
24737 &cartesian_trajectory_planner_DW.gobj_8_d,
24738 &cartesian_trajectory_planner_DW.gobj_9_g,
24739 &cartesian_trajectory_planner_DW.gobj_3_l);
24740 cartesian_tra_MATLABSystem_Init
24741 (&cartesian_trajectory_planner_B.MATLABSystem_d,
24742 &cartesian_trajectory_planner_DW.MATLABSystem_d);
24743 cartesian_tra_MATLABSystem_Init
24744 (&cartesian_trajectory_planner_B.MATLABSystem_h,
24745 &cartesian_trajectory_planner_DW.MATLABSystem_h);
24746
24747 // Start for MATLABSystem: '<S11>/Get Parameter'
24748 cartesian_trajectory_planner_DW.obj_di.matlabCodegenIsDeleted = true;
24749 cartesian_trajectory_planner_DW.obj_di.isInitialized = 0;
24750 cartesian_trajectory_planner_DW.obj_di.matlabCodegenIsDeleted = false;
24751 cartesian_trajectory_planner_DW.obj_di.isSetupComplete = false;
24752 cartesian_trajectory_planner_DW.obj_di.isInitialized = 1;
24753 for (cartesian_trajectory_planner_B.i_hp = 0;
24754 cartesian_trajectory_planner_B.i_hp < 12;
24755 cartesian_trajectory_planner_B.i_hp++) {
24756 cartesian_trajectory_planner_B.cv1[cartesian_trajectory_planner_B.i_hp] =
24757 tmp_l[cartesian_trajectory_planner_B.i_hp];
24758 }
24759
24760 cartesian_trajectory_planner_B.cv1[12] = '\x00';
24761 ParamGet_cartesian_trajectory_planner_283.initialize
24762 (cartesian_trajectory_planner_B.cv1);
24763 ParamGet_cartesian_trajectory_planner_283.initialize_error_codes(0, 1, 2, 3);
24764 ParamGet_cartesian_trajectory_planner_283.set_initial_value(40.0);
24765 cartesian_trajectory_planner_DW.obj_di.isSetupComplete = true;
24766
24767 // End of Start for MATLABSystem: '<S11>/Get Parameter'
24768
24769 // Start for MATLABSystem: '<S11>/Get Parameter1'
24770 cartesian_trajectory_planner_DW.obj_cj.matlabCodegenIsDeleted = true;
24771 cartesian_trajectory_planner_DW.obj_cj.isInitialized = 0;
24772 cartesian_trajectory_planner_DW.obj_cj.matlabCodegenIsDeleted = false;
24773 cartesian_trajectory_planner_DW.obj_cj.isSetupComplete = false;
24774 cartesian_trajectory_planner_DW.obj_cj.isInitialized = 1;
24775 cartesian_trajectory_planner_B.cv6[0] = '/';
24776 cartesian_trajectory_planner_B.cv6[1] = 't';
24777 cartesian_trajectory_planner_B.cv6[2] = 'f';
24778 cartesian_trajectory_planner_B.cv6[3] = '\x00';
24779 ParamGet_cartesian_trajectory_planner_284.initialize
24780 (cartesian_trajectory_planner_B.cv6);
24781 ParamGet_cartesian_trajectory_planner_284.initialize_error_codes(0, 1, 2, 3);
24782 ParamGet_cartesian_trajectory_planner_284.set_initial_value(70.0);
24783 cartesian_trajectory_planner_DW.obj_cj.isSetupComplete = true;
24784 }
24785}
24786
24787// Model terminate function
24788void cartesian_trajectory_planner_terminate(void)
24789{
24790 // Terminate for Atomic SubSystem: '<Root>/Subscribe'
24791 // Terminate for MATLABSystem: '<S10>/SourceBlock'
24792 matlabCodegenHandle_matla_astwh(&cartesian_trajectory_planner_DW.obj_gl);
24793
24794 // End of Terminate for SubSystem: '<Root>/Subscribe'
24795
24796 // Terminate for MATLABSystem: '<Root>/Get Parameter'
24797 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_cu);
24798
24799 // Terminate for MATLABSystem: '<Root>/Get Parameter1'
24800 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_k);
24801
24802 // Terminate for MATLABSystem: '<S15>/Get Parameter3'
24803 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_es);
24804
24805 // Terminate for MATLABSystem: '<S15>/Get Parameter4'
24806 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_f4);
24807
24808 // Terminate for MATLABSystem: '<S15>/Get Parameter5'
24809 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_en);
24810
24811 // Terminate for MATLABSystem: '<S15>/Get Parameter6'
24812 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_l0);
24813
24814 // Terminate for MATLABSystem: '<S15>/Get Parameter'
24815 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_i);
24816
24817 // Terminate for MATLABSystem: '<S15>/Get Parameter1'
24818 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_db);
24819
24820 // Terminate for MATLABSystem: '<S15>/Get Parameter2'
24821 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_oq);
24822
24823 // Terminate for MATLABSystem: '<S16>/Get Parameter3'
24824 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_e0);
24825
24826 // Terminate for MATLABSystem: '<S16>/Get Parameter4'
24827 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_a);
24828
24829 // Terminate for MATLABSystem: '<S16>/Get Parameter5'
24830 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_p);
24831
24832 // Terminate for MATLABSystem: '<S16>/Get Parameter6'
24833 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_l);
24834
24835 // Terminate for MATLABSystem: '<S16>/Get Parameter'
24836 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_mb);
24837
24838 // Terminate for MATLABSystem: '<S16>/Get Parameter1'
24839 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_c);
24840
24841 // Terminate for MATLABSystem: '<S16>/Get Parameter2'
24842 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_f);
24843
24844 // Terminate for MATLABSystem: '<S12>/Get Parameter'
24845 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_e);
24846
24847 // Terminate for MATLABSystem: '<S12>/Get Parameter1'
24848 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_d);
24849
24850 // Terminate for MATLABSystem: '<S12>/Get Parameter2'
24851 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_m);
24852
24853 // Terminate for MATLABSystem: '<S12>/Get Parameter3'
24854 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_h);
24855
24856 // Terminate for MATLABSystem: '<S12>/Get Parameter4'
24857 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_mf);
24858
24859 // Terminate for MATLABSystem: '<S12>/Get Parameter5'
24860 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_o);
24861
24862 // Terminate for MATLABSystem: '<S6>/MATLAB System'
24863 matlabCodegenHandle_matlabC_ast(&cartesian_trajectory_planner_DW.obj);
24864 matlabCodegenHandle_matlabCo_as
24865 (&cartesian_trajectory_planner_DW.obj.IKInternal);
24866 emxFreeStruct_robotics_slmani_a(&cartesian_trajectory_planner_DW.obj);
24867 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_1);
24868 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_50);
24869 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_49);
24870 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_48);
24871 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_47);
24872 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_46);
24873 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_45);
24874 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_44);
24875 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_43);
24876 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_42);
24877 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_41);
24878 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_40);
24879 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_39);
24880 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_38);
24881 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_37);
24882 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_36);
24883 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_35);
24884 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_34);
24885 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_33);
24886 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_32);
24887 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_31);
24888 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_30);
24889 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_29);
24890 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_28);
24891 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_27);
24892 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_26);
24893 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_25);
24894 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_24);
24895 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_23);
24896 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_22);
24897 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_21);
24898 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_20);
24899 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_19);
24900 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_18);
24901 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_17);
24902 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_16);
24903 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_15);
24904 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_14);
24905 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_13);
24906 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_12);
24907 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_11);
24908 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_10);
24909 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_9);
24910 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_8);
24911 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_7);
24912 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_6);
24913 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_5);
24914 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_4);
24915 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_3);
24916 emxFreeStruct_c_rigidBodyJoin_a(&cartesian_trajectory_planner_DW.gobj_2);
24917 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_51);
24918 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_82);
24919 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_81);
24920 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_80);
24921 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_79);
24922 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_78);
24923 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_77);
24924 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_76);
24925 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_75);
24926 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_74);
24927 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_73);
24928 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_72);
24929 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_71);
24930 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_70);
24931 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_69);
24932 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_68);
24933 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_67);
24934 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_66);
24935 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_65);
24936 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_64);
24937 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_63);
24938 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_62);
24939 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_61);
24940 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_60);
24941 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_59);
24942 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_58);
24943 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_57);
24944 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_56);
24945 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_55);
24946 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_54);
24947 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_53);
24948 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_52);
24949 emxFreeStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_83);
24950 emxFreeStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_84);
24951 emxFreeStruct_f_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_85);
24952 emxFreeStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_86);
24953 emxFreeStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_87);
24954 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_88);
24955 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_103);
24956 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_102);
24957 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_101);
24958 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_100);
24959 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_99);
24960 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_98);
24961 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_97);
24962 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_96);
24963 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_95);
24964 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_94);
24965 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_93);
24966 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_92);
24967 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_91);
24968 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_90);
24969 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_89);
24970 emxFreeStruct_robotics_slman_as(&cartesian_trajectory_planner_DW.obj_g);
24971 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_1_h);
24972 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_16_c);
24973 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_15_g);
24974 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_14_h);
24975 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_13_a);
24976 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_12_b);
24977 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_11_i);
24978 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_10_f);
24979 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_9_g);
24980 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_8_d);
24981 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_7_b);
24982 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_6_a);
24983 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_5_g);
24984 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_4_j);
24985 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_3_l);
24986 emxFreeStruct_n_robotics_mani_a(&cartesian_trajectory_planner_DW.gobj_2_m);
24987 cartesian_tra_MATLABSystem_Term
24988 (&cartesian_trajectory_planner_DW.MATLABSystem_d);
24989
24990 // Terminate for Atomic SubSystem: '<Root>/Publish'
24991 // Terminate for MATLABSystem: '<S9>/SinkBlock'
24992 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_n);
24993
24994 // End of Terminate for SubSystem: '<Root>/Publish'
24995 cartesian_tra_MATLABSystem_Term
24996 (&cartesian_trajectory_planner_DW.MATLABSystem_h);
24997
24998 // Terminate for MATLABSystem: '<S11>/Get Parameter'
24999 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_di);
25000
25001 // Terminate for MATLABSystem: '<S11>/Get Parameter1'
25002 matlabCodegenHandle_matlabCodeg(&cartesian_trajectory_planner_DW.obj_cj);
25003}
25004
25005//
25006// File trailer for generated code.
25007//
25008// [EOF]
25009//
25010